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This  report  describes  a  general-purpose  program  that  was  developed  to 
help  design  and  evaluate  Kalman  filters  for  integrated  systems.  The  program, 
which  is  named  SOFE,  is  a  Monte  Carlo  simulation  that  can  be  used  for  system 
performance  analysis  once  the  Kalman  filter  is  designed  and  verified.  SOFE 
is  written  in  FORTRAN  and  is  intended  for  batch  use  on  a  digital  computer. 
This  report  is  a  corplete  user's  manual  for  SOFE.  It  documents  the  equations 
used,  the  program  structure,  the  input  requirements  and  output  options,  and 
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concludes  with  excjples  of  SOFE  use  in  both  linear  and  nonlinear  (extended) 
Kalman  filter  design  studies ^ 

J  SOFTS  is  divided  into  two  modules  named  basic  SOFE  and  user -written  SOFE. 
Basic  SOFE  accanplishes  all  of  the  usual  bookkeeping  and  integration  functions 
of  a  Monte  Carlo  simulation  (see  below)  and,  in  addition,  provides  the  algo¬ 
rithm  for  measurement  update  of  the  user’s  filter.  As  its  name  inplies, 
user-written  SOFE  is  a  module  of  FORTRAN -coded  subroutines  supplied  by  the  user. 
These  routines  specify  the  system  under  study,  including  both  truth  and 
filter  components .  The  rationale  for  this  division,  which  places  the  constant 
routines  in  basic  SOFE  and  the  variable  routines  in  user-written  SOFE,  is  to 
free  the  designer  to  concentrate  on  more  central  issues  including  problem 
definition,  model  specification,  model  validation,  and  insight  acceleration 
through  experimentation. 

The  31  routines  of  basic  SOFE  perform  I/O  (including  printer  plots) , 
problem  setup,  run  setup,  numerical  integration  of  the  ordinary  differential 
equations  that  specify  the  system  dynamics,  measurement  update,  run  termination 
and  problem  termination.  Basic  SOFE  also  provides  for  consecutive  runs  that, 
taken  together,  cons  tit  ate  a  study.  User-written  SOFE  must  have  nine  FORTRAN 
routines  that  supply  derivatives,  measurements,  truth  model  fluctuations, 
trajectory  data,  etc,  ^ 

^  SOFE  is  designed  to  be  efficient  in  its  use  of  core  and  time.  Savings 
were  obtained  in  these  areas  by  sparse  matrix  methods,  by  staring  inly  the 
nonredundant  portions  of  symmetric  matrices,  by  packing  all  vepfedrs  and 
matrices  in  juxtaposition  in  a  single  array,  by  avoiding^erTSmultiplies , 

and  by  elimination  of  double  subscripts.  ,  - *" 

Sa 

SOFE  was  developed  on  a  CDC  CVBER-74  ccrputer  where  it  carpi les  in  eleven 
seconds  and  uses  74000  octal  words  of  memory  to  solve  a  small  problem.  Seme 
effort  was  made  to  adhere  to  ANSI  constructs  but  high  portability  is  " 
claimed.  A  conpanion  post-processor  program  (SOFEPL)  is  available  far  dopng 
ensemble  averaging  across  runs  and  then  making  various  pen  plots. 
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This  report  was  written  by  Stanton  H.  Musick  of  the 
Reference  Systems  Branch,  System  Avionics  Division,  Avionics 
Laboratory,  Air  Force  Wright  Aeronautical  Laboratories, 
Wright  Patterson  A FB,  Ohio. 

The  work  documented  herein  was  carried  out  under  Pro¬ 
ject  Work  Unit  1206  0120.  Most  of  this  work  occurred  in  the 
period  January  to  May  1978  and  was  documented  in  draft  form 
in  June  1978  in  A FAL-TM-78-1 9 . 

Since  June  1978,  work  has  continued  on  1206  0120  to  re¬ 
fine  both  the  S0FE  program  and  its  documentation,  contained 
herein.  This  report  constitutes  a  portion  of  the  final 
documentation  for  this  work  unit.  A  companion  report  on 
S0FEPL,  Reference  5,  documents  a  postprocessor  for  S0FE  ca¬ 
pable  of  making  line  plots. 

The  author  would  like  to  acknowledge  three  people  for 
their  assistance  in  developing,  testing  and  documenting  both 
SO FE  and  S0FEPL.  Nelson  Estes,  who  was  Involved  early  in 
the  day-to-day  work  on  the  S0FE  code,  wrote  several  1/0  rou¬ 
tines  and  helped  implement  the  external  trajectory  capabili¬ 
ty.  Richard  Feldmann,  who  replaced  Nelson  during  S0FE  de¬ 
velopment,  modified  the  sparse  matrix  processing  routines 


to  simplify  structure  and  increase  efficiency,  helped  main¬ 
tain  the  program  through  numerous  revisions,  designed  and 
built  the  plot  postprocessor  SOFEPL,  and  helped  author  the 
SOFEPL  report.  Reference  5.  Elizabeth  Ditmer  put  the 
manuscript  version  of  this  report  in  final  printed  form 
using  a  Digital  Equipment  Corporation  (DEC)  program  named 
RUNOFF.  Most  text  preparation  and  all  line  justification, 
paragraph  spacing,  table  setup,  etc.,  were  accomplished 
under  RUNOFF  while  most  figures  were  made  using  a  TEKTRONIX 
4014  graphics  terminal  interacting  with  an  in-house  program 
called  FLOWCHART,  which  ran  on  a  DEC  P DP  11/40  computer.  My 
special  thanks  to  all  of  you.  Nelson,  Dick  and  L.ibby. 
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SOFEPL  SO  FE  PLot  program 


1.0  INTRODUCTION 


S  OF  £  Is  a  Monte  Carlo  simulation  program  that  was  de¬ 
veloped  to  help  analyze  integrated  systems  that  employ  Kal¬ 
man  filter  estimation  techniques.  SOFE  should  be  useful  in 
all  phases  of  most  filter  analysis  projects,  beginning  with 
the  initial  filter  design  and  ending  with  a  full  system  per¬ 
formance  analysis. 

This  is  the  SOFE  User's  Manual.  It  is  written  primari¬ 
ly  for  people  who  already  understand  Kalman  estimation.  It 
should  help  them  shorten  the  cycle  from  filter  design 
through  filter  verification  to  performance  analysis  by  re¬ 
moving  much  of  the  mundane  programming  load  and  providing  a 
broad  range  of  options  for  viewing  performance  results.  One 
such  option  is  provided  by  an  ensemble  averaging  and  plot¬ 
ting  program  (SOFEPL)  which  Is  documented  in  a  companion  vo¬ 
lume  to  this  one.  Reference  5. 

This  section  begins  with  a  background  discussion  that 
shows  the  need  for  a  program  such  as  SOFE  and  compares  the 
Monte  Carlo  to  the  covariance  analysis  approach.  A  SOFE 
overview  then  follows  to  relate  the  purpose,  evolution  and 
nature  of  the  program.  A  brief  section-by-section  review  of 
this  report  concludes  this  section. 
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1 . 1  Background 


A  large  class  of  modern  problems  requires  the  estima¬ 
tion  (and  control)  of  the  state  of  a  dynamic  system  based  on 
noise-corrupted  observations.  These  problems  arise  in  a 
variety  of  fields  including  physics,  economics,  medicine  and 
engineering.  When  the  dynamic  system  is  continuous  and  the 
observations  discrete,  the  solution  to  the  estimation  part 
uf  such  problems  is  given  by  an  algorithm  called  the  contin¬ 
uous-discrete  Kalman  filter. 

A  Kalman  filter  is  an  optimal,  recursive,  computational 
algorithm  that  is  used  to  combine  functionally-related  meas¬ 
urements  in  order  to  estimate  desired  variables.  The  filter 
design  procedure  begin*!  with  the  development  of  mathematical 
and  statistical  models  to  describe  the  truth  system  includ¬ 
ing  system  and  measurement  dynamics,  system  disturbances  and 
measurement  errors,  and  initial  condition  information. 
These  truth  models  are  often  both  complex  and  large  and  must 
usually  be  simplified  and  reduced  in  size  for  implementation 
in  an  operational  Kalman  filter.  As  in  any  formulation  of 
mathematical  models  for  physical  processes,  the  challenge  is 
to  develop  models  for  the  filter  that  are  sufficiently  com¬ 
plete  to  represent  the  physical  phenomenon  of  interest  but 
not  so  complex  as  to  be  computationally  intractable.  Thus 
the  designer  must  build  a  'reduced-order'  filter  model  that 
is  simpler  than  his  truth  model.  This  reduced-order  filter 
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will  execute  faster  and  use  less  core  than  the  truth  model 


would  have,  advantages  which  are  crucial  in  many  applica¬ 
tions.  If  properly  designed,  the  filter  wilt  compensate  for 
the  mismodeling  errors  and  track  the  physical  phenomenon 
well  enough  to  satisfy  the  performance  criterion.  If  im¬ 
properly  designed,  the  filter  state  will  diverge  from  the 
truth  state. 

In  order  to  develop  the  required  compensation,  to  se¬ 
lect  filter  states,  to  test  modeling  alternatives,  to  study 
the  effects  of  uncertainties  in  the  truth  model,  to  test  vi¬ 
olations  of  assumptions,  to  do  performance  tradeoffs,  to 
provide  problem  insight,  and  so  forth,  computer  programs  are 
required.  Two  types  of  programs  are  commonly  used,  the  sim¬ 
ulation  and  the  covariance  analysis.  This  document  is  a 
user's  manual  for  a  simulation  named  'SOFE'  (1). 

Simulation  is  a  much  used  procedure  whereby  one  con¬ 
structs  an  experiment  on  the  computer  that  emulates  the 
truth  system  (the  environment)  and  the  filter  system  operat¬ 
ing  together.  The  mathematical  models  for  describing  the 
dynamics  of  both  the  filter  and  truth  systems  are  (possibly 
nonlinear)  stochastic  differential  equations.  In  addition, 
(possibly  nonlinear)  stochastic  algebraic  equations  model 
the  measurements.  The  random  driving  functions  for  the 

(1 )Simulation  for  Optimal  Filter  Evaluation,  pronounced 
so-f ee . 
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truth  system  as  well  as  the  noisy  measurements  are  simulated 
with  the  aid  of  random  number  generation.  These  measure¬ 
ments  are  processed  by  the  Kalman  algorithm  to  produce  up¬ 
dated  estimates  of  the  filter  state  and  its  covariance. 
Between  measurements,  the  filter  state  and  covariance  are 
propagated  by  numerical  integration.  By  making  repeated 
runs  using  different  random  number  sequences,  one  can  form 
the  appropriate  ensemble  statistics  to  determine  whether  the 
candidate  filter  design  diverges  or  tracks  the  truth  system 
within  acceptable  bounds. 

A  covariance  analysis  generates  the  second-order 
statistics  for  both  the  truth  model  and  the  filter  design 
directly  so  that  one  covariance  analysis  run  is  equivalent 
to  an  ensemble  of  Nonte  Carlo  simulation  runs.  The  poten¬ 
tial  for  computer  savings  is  obvious  but  the  technique  only 
works  under  several  strict  assumptions,  principal  among 
which  are  linearity  in  all  models  and  Gaussian  random 
processes.  Often  covariance  analysis  Is  most  appropriate  In 
the  early  phase  of  a  project  when  these  assumptions  are 
tolerated  for  the  purpose  of  making  a  preliminary  design  or 
performance  prediction.  A  simulation  can  be  used  to  study 
many  aspects  of  a  filter  design  problem  that  covariance  ana¬ 
lysis  cannot  handle  and  Is  therefore  a  natural  next  step  In 
filter  development  and  performance  analysis. 
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1.2  S  OF  E  Overview 


S OF E  is  an  efficient,  general-purpose,  Monte  Carlo  sim¬ 
ulation  program  for  analyzing  integrated  systems  that  employ 
Kalman  estimation  techniques.  SOFE  is  a  simulation  in  the 
sense  described  above,  i.e.,  it  provides  a  means  for  con¬ 
structing  a  truth  system  and  for  testing  a  filter  to  track 
that  system  in  a  series  of  computer  experiments.  The  truth 
system,  represented  by  a  model  whose  state  vector  is  denoted 
Xa,  is  described  by  a  set  of  stochastic  differential  equa¬ 
tions,  supplied  by  the  user,  that  emulate  the  real  world 
with  its  attendant  random  qualities.  The  filter  system  is 
also  described  by  differential  equations,  again 
user-supplied.  The  filter  state  vector  and  its  error  covar¬ 
iance  are  denoted  Xf  and  Pf  respectively. 

SOFE  is  general-purpose  in  the  sense  that  all  types  of 
filter  design  problems  can  be  studied.  This  occurs  because 
the  user  describes  his  particular  problem  through  a  set  of 
nine  subroutines  that  he  writes  and  appends  to  the  basic 
SOFE  program.  No  assumptions  are  made  in  the  basic  SOFE 
program  that  particularize  it  to  a  single  problem. 

SOFE  is  efficient  in  its  use  of  core  and  computing 
time.  Since  Pf  is  symmetric  and  S  (  Pf»S*ST)  is  upper  tri¬ 
angular,  it  Is  sufficient  to  work  with  only  those  elements 
of  Pf  and  S  on  and  above  the  diagonals.  Thus  both  Pf  and  S, 
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which  are  square  matrices  of  order  NF  ,  are  stored  and  ad¬ 
dressed  as  Linear  arrays  of  Length  NF(NF*1)/2.  This  costs 
something  in  programming  complexity  but  yields  storage  sav¬ 
ings  for  these  two  arrays  approaching  50%  in  high-order  de¬ 
signs.  Further  core  savings  are  obtained  by  dense  packing 
of  all  vectors  and  matrices  in  a  single  array  that  may  be 
conveniently  contracted  or  expanded  to  better  fit  the  user’s 
problem.  Computing  time  savings  accrue  from  increased  use 
of  singly  subscripted  arrays  and  elimination  of  all  zero 
multiplies  in  forming  the  derivative  of  Pf. 

SOFE  is  an  outgrowth  of  two  other  programs,  GCAP  and 
MCAP,  References  1  and  2.  GCAP  is  a  covariance  analysis 
program  that  uses  sparse  matrix  storage  and  efficient  matrix 
manipulation  techniques  to  save  computer  space  and  execution 
time.  SOFE  has  borrowed  and  improved  on  both  of  these  fea¬ 
tures  to  achieve  similar  advantages.  MCAP  on  the  other  hand 
is  a  true  Monte  Carlo  simulation  (like  SOFE)  that  was  con¬ 
structed  by  melding  many  of  the  routines  from  GCAP  with  a 
revised  executive  and  some  new  routines  for  handling  state 
vector  propag at i on/ update .  Both  GCAP  and  MCAP  have  several 
prominent  deficiencies:  both  use  a  fixed  step  integrator 
that  lacks  error  control;  MCAP  propagates  Xs,  Xf  and  Pf  as 
if  they  were  independent  when  in  general  they  are  not;  both 
use  the  standard  Kalman  filter  update  equations  which  can 
lead  to  negative  covariances;  both  restrict  the  size  of  the 
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problem  that  can  be  worked.  SOFE  corrects  these  deficien¬ 
cies  and  adds  some  new  capabilities  including  printer  plot¬ 
ting,  line  (pen)  plotting  as  is  done  using  a  line  plotter  or 
a  graphics  terminal,  the  ability  to  acquire  and  interpolate 
data  from  an  external  trajectory  tape,  and  validation  of  the 
user's  input  data.  In  addition,  SOFE  formats  and  limits  the 
amount  of  output  on  each  printed  page,  centralizes  the  con¬ 
trol  of  output  In  a  single  routine,  provides  a  standard 
check  number  at  run  conclusion,  uses  a  very  compact  struc¬ 
ture  for  packing  the  vectors  and  matrices  of  the  problem  in 
blank  COMMON,  and  adheres  to  ANSI  constructs  insofar  as  pos¬ 
sible. 

SOFE  is  written  in  FORTRAN  using  only  single  precision 
quantities.  It  was  developed  on  a  Control  Data  Corporation 
CYBER  74  computer  where  it  executes  in  batch  mode.  It  con¬ 
sists  of  a  main  program,  29  subprograms  and  a  block  data 
routine  which  together  are  called  'basic  SOFE'.  A  complete 
load  module  consists  of  basic  SOFE  plus  nine  user-written 
routines.  Basic  SOFE  loads  in  70000  octal  words  of  memory 
on  the  CDC  system.  A  sample  problem  consisting  of  9  truth 
states  and  5  filter  states  (Section  5.1)  boosts  the  total 
memory  requirement  to  74000  octal  words. 

A  reasonable  effort  has  been  made  to  use  conventions, 
ANSI  standard  constructs  and  modular  concepts  that  will  ex- 
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pedite  SOFE's  transfer  to  other  machines.  SOFE  uses  several 
routines  fro*  the  standard  FORTRAN  *ath  library  (SORT,  AB  S, 
AMIN1,  etc.)  plus  five  special  CDC  library  routines:  DATE, 
TIME,  EOF,  RANSET  and  RANF  .  SOFE  requires  these 
peripherals:  card  reader  or  data  Input  terminal;  line 
printer;  eight  (local)  files  for  data  I/O.  In  addition,  if 
line  plots  are  desired,  the  user  will  need  the  DISSPLA  post¬ 
processor  software  and  a  computer  graphics  device. 

1 . 3  Scope 

This  report  Is  a  user's  manual  for  SOFE.  Me  will  try 
to  acquaint  the  user  with  all  its  attributes  and  limitations 
and,  by  exampte,  show  him  how  It  operates.  However,  this 
will  not  be  a  training  manual  for  Kalman  filter  design. 
Such  an  effort  Is  considerably  beyond  the  scope  of  this  re¬ 
port. 


Section  2  presents  the  propagation  and  square  root  up¬ 
date  equations  that  form  the  mathematical  basis  for  SOFE's 
design.  Section  3  describes  the  program,  giving  information 
about  Its  structure  and  coding  conventions.  Section  4  spec¬ 
ifies  the  format  and  content  of  program  Inputs,  outputs  and 
user-written  routines.  Section  5  presents  two  example  Kal¬ 
man  filter  design  problems,  one  linear  and  one  nonlinear. 
Appendices  A,  B  and  C  give  samples  of  output  from  these  de¬ 
signs.  Appendix  D  covers  job  control  for  attaching,  compll- 
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1  ng ,  loading  and  executing  SOFE  on  the  COC  coaputer  under 
the  NOS/BE  operating  system. 
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2.0  SIMULATION  MATHEMATICS 

The  mathematics  underlying  the  SOFE  simulation  consists 
of  general  models  for  the  truth  and  filter  systems  together 
with  algorithms  for  the  extended  Kalman  filter.  All  of 
these  models  and  algorithms  are  presented  in  this  section, 
along  with  some  discussion  of  numerical  techniques  that  are 
used  in  the  basic  SOFE  code.  The  section  emphasizes  what 
'data'  the  user  must  supply,  via  his  own  code,  to  effect  a 
solution  of  the  Kalman  algorithms. 

Me  assume  physical  processes  that  are  inherently  con¬ 
tinuous  and  measurements  that  are  discrete,  a  combination 
that  leads  to  the  so-called  'continuous  discrete  Kalman 
filter'.  Purely  continuous  systems  or  other 
d i sc rete- cont 1 nuous  combinations  are  not  addressed.  Since 
the  literature  abounds  with  good  material  on  the  Kalman 
filter  equations,  e.g.  Reference  6,  we  present  them  here 
with  some  explanation  but  without  derivation. 

A  DEC  program  named  RUNOFF  was  used  to  produce  this 
document  on  a  computer  printer.  Since  RUNOFF  cannot  gener¬ 
ate  subscripts  or  superscripts,  some  compromises  must  be 
used,  especially  when  writing  equations.  In  particular,  we 
chose  to  use  trailing  letters  Instead  of  subscripts  (e.g. 
ti  Instead  of  t*  )  and  to  Insert  superscripts  manually 
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only  one  6reek 


(e.g.  In  X.f  the  “  Is  so  entered)*  Also, 
letter  is  used  (a  concocted  ®)  and  special  symbols  such  as 
overbars  are  employed  sparingly. 

Note  that  a  trailing  's*  on  a  symbol  (e.g.  £s,  vs,  vs) 

marks  that  symbol  as  a  truth  model  quantity.  Similarly,  a 
trailing  'f*  on  a  symbol  (e.g.  X_f,  Qf,  h^f)  marks  it  as  a 
filter  model  quantity.  ’s’  uas  used  instead  of  't'  for 
truth  model  quantities  because  't*  is  reserved  for  time. 
The  s-quantities  may  be  thought  of  as  standards  against 
which  f-quantities  will  be  compared. 

To  distinguish  between  matrices,  vectors  and  scalars, 
matrices  are  denoted  by  a  leading  upper-case  letter  that  is 
not  underlined,  e.g.  F,  H,  Qs.  Vectors  are  denoted  by 
upper-  or  lower-case  letters  and  are  always  underlined,  e.g. 
Xs,  ws,  f/.).  Scalars  are  either  upper-  or  lower-case  and 
are  not  underlined,  e.g.  i,  j,  t,  Ai.  This  scheme  allows 
one  to  recognize  a  vector  immediately  by  its  underline,  but 
forces  one  to  distinguish  a  matrix  from  an  uppercase  scalar 
by  the  context  of  its  use. 

2.1  Truth  Wode  l 

A  quite  general  representation  of  a  dynamic, 
continuous-time,  physical  system  is  given  by  the  following 
vector,  stochastic,  ordinary  differential  equation: 
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XS  (t  )  *  £(X^,t)  ♦  WS(t) 


(2-1  ) 


where 

t  Is  time 

Xs(t)  is  the  truth  system  state  vector  (NSxl) 

£(.)  is  the  truth  system  dynamics  vector  (NSxl) 
ws(t)  is  a  zero-mean  white  Gaussian  ran¬ 
dom  process  of  dimension  NSxl  with 
ECws(t)wsT(t+T>>  =  Qs  ( t  )  *  S(T  ) 

Qs(t)  is  the  truth  system  noise  strength  (NSxNS) 

and  where,  for  an  initial  time  to,  X^s(to)  is  a  random  vec¬ 
tor,  independent  of  ws(t),  distributed  as  a  zero-mean  Gaus¬ 
sian  variable.  This  initial  value  is  denoted  X,so.  In  the 
above,  5(T)  is  the  Dirac  delta  function  and  E  is  the  ex¬ 
pected  value  operator. 

The  discrete-time  vector  output  _Z s  C t i  >  for  the  system 
represented  by  (2-1)  is  modeled  as 

Zs(ti)  *  hs(Xs,t1)  ♦  vs(ti)  (2-2) 

where 

tl  Is  a  discrete  measurement  time,  1*1, 2, 3,... 

Zs(ti)  Is  the  measurement  vector  (Rxl) 
hs(.)  is  the  measurement  model  function  (Hxl) 
vs(tl)  Is  an  ffxl  zero-mean  white  6auss1an  random 
sequence  Independent  of  both  £S(t)  and  Xso  with 
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E  {  vs  ( t  i  )  v  sT(  t  j  )  >  =  R  s  (  t  i  )  *  5i  j 

where  Rs  is  the  MxW  measurement  noise  matrix  and 
5ij  is  the  Kronecker  delta  function 

Together  equations  (2-1)  and  (2-2)  form  the  most 
oetailed  model  of  the  truth  (meaning  actual)  system.  Since 
most  physical  systems  of  any  complexity  are  nonlinear  in  na¬ 
ture,  both  functions  £()^s,t)  and  £s(Xs,ti)  are  potentially 
nonlinear  in  *s,  a  fact  connoted  here  by  the  inclusion  of  X  s 
in  the  argument  lists  of  £(.)  and  h_s  ( . )  .  X.s  is  the  physical 
process  that  the  filter  will  attempt  to  track.  SOFE  solves 
the  differential  equation  in  (2-1)  in  two  stages. 

In  the  first  stage  the  homogeneous  part  of  (2-1),  name¬ 
ly  dX^s/dt  =  £()<$, t>,  is  propagated  over  a  time  interval 
(DTNOYS)  specified  by  the  user.  This  propagation  occurs  by 
means  of  a  fifth-  order  numerical  integrator  provided  in 
SOFE.  The  user  supplies  the  function  £(.)  in  subroutine 
XSDOT,  and  the  numerical  integration  occurs  automatically. 

In  the  second  stage,  propagation  has  just  concluded  and 
the  accumulated  effect  of  ws(t)  must  be  accounted  for.  A 
typical  method  for  doing  this  begins  by  computing  the  fol¬ 
lowing  ' de 1 1 a- cova r i ance  '  matrix: 

Qd  <  t  j  >  =  Qs (t j ) *DTN0YS  (2-3) 
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If  DTNOYS  is  small  compared  to  the  Shannon  sampling  per  od 
in  £(.),  Qd(tj)  approximates  the  growth  in  covariance  of  the 
X^s  process  caused  by  the  random  system  disturbance  vs(t)  on 
the  interval  DTNOYS  from  tj  to  t j ♦  i  .  Random  noise  is  in¬ 
jected  in  Xs  by  generating  a  multivariate  Gaussian  sample 
wd(tj)  having  covariance  Qd(tj),  and  then  adding  this  sample 
directly  to  X.s  •  These  actions  occur  in  SNOYS,  a 
user-written  subroutine  that  SOFE  calls  at  DTNOYS  intervals. 
Function  subroutine  GAUSS  is  provided  in  basic  SOFE  for  gen¬ 
erating  uncorrelated  random  Gaussian  samples  of  specified 
mean  and  variance.  Correlated  random  Gaussian  samples  can 
also  be  generated  (for  Qd(tj)  nondiagonal)  by  using  basic 
SOFE  subroutines  PSQRT  and  GAUSS  in  conjunction  with  one 
another  (e.g.  see  C63,  p.  A 08,  problem  7.14). 

In  some  situations  the  approximation  in  (2-3)  is  inef¬ 
ficient  and/or  inaccurate.  Inefficiencies  occur  when  the 
Shannon  sampling  period  of  £(.)  varies  significantly  during 
a  run.  In  this  situation  DTNOYS,  which  is  fixed,  must  be 
set  small  to  accommodate  the  shortest  sampling  period  in 
£(.).  But  a  small  DTNOYS  forces  a  large  number  of  interrup¬ 
tions  in  the  integration  process  and  raises  the  computation 
time.  Attempting  to  correct  the  problem  by  simply  enlarging 
DTNOYS  without  changing  (2-3)  will  eventually  lead  to  inac¬ 
curate  realizations  of  the  Xs  process.  In  such  situations 
other  options  should  be  considered  for  computing  Qd(tj). 
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If  £(.)  is  Linear  in  X.s,  several  options  surface  imme¬ 
diately.  In  this  case  (2-1)  is  written 

X^s  (t)  =  G (t) Xs  (t)  ♦  ws  (t) 

With  linear  dynamics,  methods  for  improving  the  numerical 
approximation  in  (2-3)  are  available  (e.g.  C6D,  Subsection 
6.11)  and  can  be  used  effectively  in  a  practical  filter  im¬ 
plementation.  For  a  simulation  truth  model,  however,  the 
exact  representation  for  Qd(tj)  is  of  greater  interest.  For 
the  linear  case,  Qd(tj)  may  be  computed  exactly  by  solving 
the  following  ordinary  differential  equation  for  Q(t,tj). 

i  a  T 

Q  ( t , t  j )  =  G(t)Q(t,tj)  +  Q(t,tj)G(t)  ♦  Qs(t) 

Q(tj,tj)  =  0 

Qd(tj )  =  a(tj+i,t>) 

To  implement  this  solution  in  SOFE,  one  would  include 
Q  ( t , t  j )  in  the  state  vector  Xs  and  use  SNOYS  to  formulate 
wd(tj)  from  Qd(tj)  as  described  above.  Before  exiting  from 
SNOYS,  Q(  t j -M  , t j )  would  be  reset  to  zero  to  initialize  the 
next  Integration  from  tj+i  to  tj+2.  Operating  in  this 
manner  wilt  allow  DTNOYS  to  be  set  larger  than  if  (2-3)  is 
used  with  assurances  that  dynamics  G(t)  and  plant  noise 
ws(t)  are  coupled  accurately.  As  a  practical  matter, 
however,  this  approach  is  significantly  more  complex  than 


16 


C2-3)  so  It  may  not  often  be  worth  the  effort. 

If  £(.)  is  truly  nonlinear  in  £s,  the  problem  of  a  more 
efficient  computation  for  Qd(tj)  than  (2-3)  is  even  more 
complex,  involving  stochastic  integrals  of  products  of  £(.) 
and  £s(t),  and  is  well  beyond  the  scope  of  what  can  be  dis¬ 
cussed  here. 

The  knowledgeable  reader  will  note  that  us(t),  the  ad¬ 
ditive  deterministic  forcing  function,  is  omitted  from 
(2-1).  This  omission  was  made  in  order  to  simplify  the  pre¬ 
sentation.  If  us  ( t )  were  present  in  the  user's  problem,  it 
would  be  accounted  for  like  £(  . )  is,  namely  by  including  it 
as  an  additional  term  in  the  derivatives  specified  in 
user-subroutine  XSDOT.  Basic  SOFE  would  not  need  altera¬ 
tion.  Similar  statements  apply  for  the  omission  of  uf(t) 
from  the  filter  state  differential  equation,  (2-4),  to  be 
discussed  next. 

2 . 2  Filter  Mode l 

The  continuous-time  physical  system  and  its 
di sc rete-t ime  measurement  output  are  modeled  for  the  filter 
by  these  two  (possibly  nonlinear)  equations: 

Xf(t)  ■  f/Xf,t>  ♦  wf(t>  (2-4) 
Zf  ( 1 1 >  «  hf(Xf,t1>  ♦  vf(tl)  (2-5) 
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where 


t  and  ti  are  defined  in  (2-1)  and  (2-2) 

Xf(t)  is  the  filter  state  vector  (NFxl) 
f_(.)  is  the  filter  dynamics  vector  (NFxl) 

^f(t)  Is  a  zero-mean  white  Gaussian  random 

process  of  dimension  NFxl  with 

T 

E-Cwf  (t)wf  (t  +  T)>  =  Qf(t)*S(T) 

Qf(t)  is  the  filter  noise  strength  (NFxNF) 

Z^f(ti)  is  the  measurement  vector  (Hxl) 
h_f(.)  is  the  measurement  model  function  (Nxl) 

^f(ti)  is  an  Hxl  zero-mean  white  Gaussian  random 
sequence  Independent  of  wf(t)  with 
ECvf  (ti)vfT(tj)>  =  Rf(ti)*5ij 
where  Rf  is  the  HxH  measurement  noise  matrix 

and  where  J(f(to)  is  a  zero-mean  Gaussian  random  vector,  in¬ 
dependent  of  both  wf  (t)  and  vf(ti),  and  denoted  X^fo. 

In  general,  (2-4)  and  (2-5)  for  the  filter  are  not 
identical  to  (2-1)  and  (2-2)  for  the  truth  because  of  the 
necessity  to  construct  the  filter  as  a  reduced-order  system 
suitable  to  real-time  solution.  Thus  NF  is  usually  less 
than  NS,  M.)l*£<.)  and  hf(.)^hs(.).  Obviously  the  white 
Gaussian  noise  terms  are  not  equal,  filter  to  truth,  end 
their  strengths  may  not  be  matched  either,  i.e.  Rf  may  not 
match  Rs  nor  Of  match  Qs. 
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Note  that  in  (2-4)  and  (2-5)  we  are  not  yet  dealing 
with  filter  estimates  or  actual  measurements  but  with  under¬ 
lying  models  that  will  eventually  lead  us  to  estimates  based 
on  measurements. 

2.3  Convent i ona  L  Kalman  F i  Iter  Equation  Summa  ry 

The  discrete  Kalman  filter  is  a  recursive  data  process¬ 
ing  algorithm  usually  implemented  in  software  on  a  digital 
computer.  At  update  time,  it  combines  available  measure¬ 
ments  plus  prior  knowledge  about  the  system  and  the  measur¬ 
ing  devices  to  produce  an  estimate  of  the  state  Xf  in  such 
a  manner  that  the  mean  square  error  is  minimized  statisti¬ 
cally.  During  propagation,  it  advances  the  estimate  in  such 
a  way  as  to  again  maintain  optimality. 

The  coventional  Kalman  filter  performs  the  above  tasks 
for  LI near  systems  and  linear  me asur ement s  in  which  the 
driving  and  measurement  noises  are  assumed  to  be  mutually 
uncorrelated,  white,  zero-mean  and  Gaussian,  and  the  initial 
conditions  are  Independent,  zero-mean  and  Gaussian.  These 
are  precisely  the  assumptions  made  for  the  filter  model 
given  by  equations  (2-4)  and  (2-5),  except  that  £(.)  and 
£f(.)  may  not  be  linear  in  Xf .  When  the  system  dynamics  and 
measurement  relationships  are  linear  in  X_f,  (2-4)  and  (2-5) 
can  be  rewritten  as 
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Xf(t)  =  F(t)Xf(t)  ♦  wf(t)  (2-6) 

Zf(ti)  =  H(ti)Xf(ti>  ♦  vf(ti)  (2-7) 

where  the  assumptions  regarding  noises  and  initial  condi¬ 
tions  remain  those  listed  with  (2-4)  and  (2-5). 

Now  define  )<f  as  the  estimate  of  J^f,  specifically  the 
conditional  mean,  conditioned  on  the  history  of  measurements 
taken  up  to  the  present  time.  The  error  covariance  of  *1 , 
termed  Pf,  is  the  expected  value  of  the  error  in  this  esti¬ 
mate. 


Pf  =  E{(Xf-Xf ) (Xf-Xf >T>  (2-8) 

The  Kalman  estimation  equations  appropriate  for  the 
system  in  (2-6)  and  (2-7)  are  summarized  in  Figure  2-1. 
Note  that  the  superscripts  -  and  *  on  Xf  and  Pf  refer 
respectively  to  before  and  after  measurement  Incorporation 
at  ti.  Also  note  that  the  tilde  "  over  Zs  in  (2-12)  denotes 
a  realized  value  from  the  measurement  truth  model. 
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2.4  Extended  Kalman  Filter  Formulation 


The  extended  Kalman  filter  is  a  variation  of  the  con¬ 
ventional  filter  which  relaxes  the  requirement  that  the  sys¬ 
tem  and  measurements  be  linear.  It  is  the  filter  generally 
used  in  practice  for  nonlinear  applications.  This  subsec¬ 
tion  presents  the  extended  filter  equations  as  a  logical  ex¬ 
tension  of  the  conventional  equations. 

For  £(.)  or  h_f  C . )  nonlinear  in  X_f  ,  define  these  partial 
derivatives 


F  <t ; xf  )  * 

d_f  <Xf,t)/dXf 

(2-16) 

H(ti;Xf)  * 

dhf <Xf,ti>/dXf 

(2-17) 

where  the  differentiation  is  'row-type*  meaning  that  the 
derivative  of  a  scalar  with  respect  to  a  column  vector  is  a 
row  vector.  This  produces  dimensions  for  F(.)  and  H(.)  of 
NFxNF  and  MxNF  respectively.  F(.)  and  H(.)  may  be  viewed  as 
sensitivity  matrices  that  relate  small  perturbations  in  Xf 
to  changes  in  ii  and  Zf  as  in  the  differential  calculus. 
F ( . )  is  called  the  'filter  dynamics  partial  matrix'  and  H(.) 
the  'measurement  sensitivity  matrix'.  Define  the  perturba- 

a 

tlon  D£  of  £f  from  its  current  estimate  Xf. 

DX  ^  Xf  -  Xf  (2-18) 
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The  perturbation  D£  is  called  the  error  state  while  tf  is 
the  full  state. 

Expand  ti  and  Zf  from  (2-4)  and  (2-5)  in  Taytor  series 
expansions  about  Xf  in  powers  of  t^X.  After  truncating  DX  »DX 
and  all  higher  powers  of  DJ<^  from  the  resulting  expansions# 
one  arrives  at  the  following  linearized  perturbation  equa¬ 
tions  in  DX . 

D£(t)  =  F(t;Xf )DX(t)  ♦  wf(t)  (2-19) 

DZ(ti)  =  H(ti;Xf)DX(ti)  +  vf(ti)  (2-20) 

In  (2-19)  and  (2-20)  we  have  equations  that  meet  the  assump¬ 
tions  of  the  conventional  filter.  Thus,  a  direct  estimate 
DX_+  of  the  error  state  DJ<_  can  be  made  from  measurements 
DZ^(ti)  using  equations  (2-11)  through  (2-13).  The  measure¬ 
ment  difference  DZ  (t  i )  is  called  the  residual.  It  is  formed 
in  this  case  by  subtracting  the  actual  (Zs)  and  predicted 

a 

( Z_f )  measurements. 

DZ(ti) 

where 

Z  s  ( 1 1 ) 

Zf  (ti) 


*  Zs(ti)  -  Zf(ti) 


(2-21) 


*  hs(Xs,t1)  +  vs (ti) 
»  hf(Xf7tTi 

*  h  f  (Xf , t  i ) 


(2-22) 


(2-23) 
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With  D£+  in  hand,  (2-18)  can  be  turned  around  to  yield  an 
updated  full-state  vector. 

Xf  +  3  xf“  ♦  0£+  (2-24) 

Equation  (2-24)  folds  all  the  available  data  Into  a  single 
full-state  estimate  and  thereby  allows  DJ^(t1+)  to  be  reset 
to  zero.  Returning  to  (2-19)  and  taking  the  expected  value 
of  both  sides,  we  see  that,  with  a  2ero  Initial  condition, 
0£(t)  will  be  zero  over  the  entire  Interval  between  updates. 

Mm  ■  0  for  t1+  <  t  <  tl+i”  (2-25) 

With  DX_(t  1~)  zero,  the  error-state  update  equation  (based  on 
(2-12))  simplifies  to  IM+=KD^,  which  on  substitution  In  the 
full-state  update  equation  (2-24)  produces 

Xf(t1+)  =  Xf (t 1”)  ♦  K(ti)M(t1)  (2-26) 

where  DMtl)  Is  given  by  (2-21). 


To  obtain  an  equation  for  the  propagation  of  Xf  between 
updates,  take  the  expectation  of  Taylor's  expansion  of  (2-4) 
retaining  only  the  first  term. 

X.f(t)  ■  f  (Xf,t) 

•  f  (Xf,t>  (2-27) 
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Note  that  the  form  of  ^( .  >  In  (2-27)  Is  identical  to  that  in 
(2-4)  so  that  none  of  the  dynamic  non  l  i nea r i t i es  are  lost 
during  propagation. 

In  (2-27)  we  have  arrived  at  the  desired  equation  for 
propagation  of  X^f .  Derivatives  Xf  are  supplied  to  SOFE 
through  user-routine  XFDOT,  and  SOFE’s  fifth-order  numerical 
integrator  solves  (2-27)  for  Xf  with  initial  conditions 
after  each  update  being  those  produced  by  (2-26).  Equation 
(2-26)  not  only  updates  the  full  state  vector  but,  in  ef¬ 
fect,  relineari2es  the  state  around  a  new  nominal  that  en¬ 
hances  the  validity  of  (2-19)  and  (2-20)  for  the  next  propa¬ 
gation.  Although  (2-26)  could  serve  as  written,  it  will  be 
replaced  by  a  numerically  superior  method  as  discussed  in 
the  next  subsection. 

Consider  now  the  question  of  the  error  covariance  Pf  of 
the  full-state  Xf  and  its  relationship  to  the  error  covari¬ 
ance  Pfd  of  DX . 

Pf d  *  E<£X-DX) (DX-£x)T> 

»  E<DX  DXT>  by  (2-25) 

«  E<<Xf-)Tf  >  (Xf-Xf  >T>  by  (2-18) 

■  by  (2-8) 

In  words,  the  error  covariance  of  Xf  is  identical  to  that  of 
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DX .  But  DJ^’s  covariance  is  given  by  conventional  filter 
equation  (2-10)  with  F(t;£f)  replacing  F(t).  Thus 

Pf(t)  =  F(t;Xf)Pf(t)  ♦  Pf  (t )  F^Xf  )  flf(t)  (2-28) 

Equation  (2-28)  governs  the  evolution  of  Pf  between  up¬ 
dates.  For  measurement  update  of  Pf,  (2-13)  could  serve, 
but  a  numerically  superior  algorithm  exists  and  is  imple¬ 
mented  in  SOFE.  The  next  subsection  presents  that 
algorithm. 

2.5  Square  Root  Update  Algorithm 

The  measurement  update  formulation  used  in  SOFE  is  the 
sequential  square  root  form  developed  by  Carlson  and  docu¬ 
mented  in  C33.  Carlson's  approach  is  algebraically  equiva¬ 
lent  to  the  standard  approach  of  (2-11)  through  (2-13)  if 
these  standard  equations  are  used  in  a  'recursive  scalar  up¬ 
date  mode'.  In  this  mode  Hj+1  and  Zfj+1,  the  measurement 
sensitivity  (row)  vector  and  the  predicted  value  for  the 
j  ♦I  t_h  of  M  simultaneous  measurements,  are  computed  based  on 
Xf+J,  the  state  estimate  available  after  j  scalar  measure¬ 
ments  have  been  incorporated  iteratively.  In  nonlinear 
problems  this  recursive  re 1 1  near i za 1 1  on  between  measurements 
yields  Improved  estimates  of  J(f  +and  Pf+whether  Carlson 
square  root  or  standard  equations  are  used.  However,  the 
Carlson  form  offers  several  additional  numerical  advantages 
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on  finite  wordlength  computers  C6,  p  3993: 


o  It  is  numerically  stable  whereas  the  standard  fora  is 
unstable. 

o  It  is  approximately  twice  as  precise  as  the  standard 
fora. 


o  It  effectively  guarantees  a  nonnegative  square  root 
matrix  S+. 

The  cost  of  these  advantages,  a  modest  increase  in  computa¬ 
tion  time  and  required  storage,  is  judged  small  compared  to 
their  benefit. 

This  section  summarizes  the  Carlson  update  equations 
used  in  SOFE.  Note  that  the  time-propagation  equations  dis¬ 
cussed  previously  are  not  those  suggested  by  Carlson  in  C33. 
However,  the  two  time-propagation  approaches  are  alike  in 
the  essential  fact  that  both  propagate  Pf  Instead  of  its 
square  root  S.  For  notatlonal  simplicity,  we  suppress  time 
arguments  in  this  subsection. 

The  error  covariance  square  root  matrix  S  is  related  to 

Pf  by 


Pf  =  s*sF 


(2-29) 


To  make  S  unique,  Carlson  chooses  the  upper  triangular  form 
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obtained  by  Cholesky  decomposition  of  Pf.  The  update  pro¬ 
cess  begins  by  computing  S  from  Pf  ,  the  error  covariance 
available  at  the  end  of  time  propagation.  Such  an  S-  can 
always  be  found  if  Pf""  is  positive  semidefinite  (e.g.C43, 
p.81).  Next/  each  measurement  is  processed  individually  by 
the  update  algorithm  to  produce  from  the  original  S-  and 
Xi~  ,  updated  versions  denoted  S+and  )^f+.  Finally,  S+  is 
'squared'  using  (2-29)  to  re-form  Pf+  ,  and  the 
t 1 me-pr op aga t i on  procedure  is  ready  to  resume.  The  equa¬ 
tions  for  this  sequence  are  detailed  below. 


Begin  update  by  computing  the  Cholesky  square  root  S  , 
denoted  symbolically  as  follows: 


S~  a 


(2-30) 


For  the  details  of  the  computations  in  (2-30),  see  Reference 
6,  page  372.  For  each  realized  measurement  Zsj,  j  = 
1,2,. ..,N,  perform  the  following  sequence: 


d  *  (S~ )TH j T 
Ao  =  Rfj 
bo  ■  0 

Repeat  for  i  *  1  to  NF  <2-31) 

k  »  1-1 
A1  «  Ak  ♦  dl  2 
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« 


b  i  =  bk  ♦  S_i~  di 
S  i  +  =  (sT-bkdWAk)  (Ak/Ai)  1/2 
Xf  +  =  Xf"  ♦  (b^  /A  Mr )  DZ  j  (2-32) 

where 


Hj  =  iaeasure«ent  gradient  vector/  the  jt_h  (2-33) 
row  of  H(ti;_Xf)  from  (2-17) 

Rfj  =  measurement  error  variance,  the  j,jt_h  (2-34) 
element  of  diagonal  Rf 
di  =  i t h  element  of  d 
S^i  “  =  i  t_h  column  of  $" 

DZj  *  measurement  residual,  the  j  t_h^  element  (2-35) 
of  DZ(ti)  from  (2-21) 


Where  X.*  data  is  required  to  evaluate  Hj  or  DZj,  the  esti¬ 
mate  through  j-1  iterations  should  be  used  since  it  is  the 
best  available  data. 


Note  the  similarity  of  the  X.f  update  equation  in 
(2-26)  to  its  replacement  (2-32),  the  gain  K  for  the  vector 
measurement  residual  DZ  (t  i )  corresponding  to  for  the 
scalar  measurement  residual  DZj.  When  all  N  measurements 
have  been  processed  through  (2-31)  and  (2-32),  X^f  and  S  are 
fully  updated  and  Pf  can  be  re-formed  using  (2-29). 

When  the  M  measurements  are  uncorrelated,  Rf  is  diago¬ 
nal  and  the  above  procedure  goes  through  directly.  When  the 
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M  measurements  are  instead  correlated,  Rf  is  not  diagonal 
and  the  following  linear  transformation  is  recommended  to 
provide  N  un correlated  measurement  combinations  DZ': 


V  =  «(l/2 
V*cTz  *  =  OZ 

V* H  *  =  H 

R '  =  I 


- >  ^Z  ' 

- >  H  ' 


If  V  is  obtained  as  the  Cholesky  square  root  of  Rf,  then  DZ ' 
and  H'  can  be  obtained  by  back  substitution  without  invert¬ 
ing  V . 

2.6  Summa  ry  of  Extended  Fi  Iter  Equations 

Equations  (2-27)  through  (2-32)  form  the  system  of 
equations  comprising  the  extended  Kalman  filter  as  imple¬ 
mented  in  SOFE.  These  are  the  equations  incorporated  in  the 
basic  SOFE  code.  Through  his  subroutines,  the  user  supplies 
f(.),  F  ( . ) ,  Qf ( . ) ,  Hj,  Rfj,  and  DZj  to  effect  the  solution 
of  these  imbedded  equations.  A  summary  of  equations  (2-27) 
through  (2-32),  together  with  the  necessary  supporting  for¬ 
mulas,  is  given  in  Figure  2-2. 

Mote  that  SOFE  makes  no  distinction  between  a  linear 
and  a  nonlinear  problem.  During  propagation,  SOFE  only 
knows  it  must  numerically  integrate  D.E.s  for  the  truth 


30 


model,  the  filter  model  and  the  covariance.  It  cannot  tell 
linear  D.E.s  from  nonlinear  ones.  During  update,  SOFE  per¬ 
forms  algebraic  operations  on  Pf  and  using  Hj,  Rfj  and 
DZj  data  supplied  by  the  user-written  subroutine  HRZ.  It  is 
true  that  Hj  and  DZj  may  depend  on  *1 ,  but  this  does  not 
alter  the  form  of  SOFE's  internal  algebra.  Moreover,  since 
DZj  is  just  a  scalar  residual  to  (2-32)  --  DZ  j  =Zs  j -H  jj(f 

for  linear  problems  or  DZj=Zsj-Zfj  for  nonlinear  problems 
the  linear/nonlinear  problem  structures  are  again  masked 
from  view  in  SOFE.  In  short,  SOFE  is  equally  applicable  to 
linear  problems  and  to  nonlinear  problems  that  employ  ex¬ 
tended  filter  design  principles. 
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N  UPDATE 


INITIAL  CONDITIONS  AT  to 


2.7  Feedback  Control 


After  update/  impulsive  changes  in  £f  +  and  Xs  can  be 
applied  as  the  user  desires  through  subroutine  AMEND.  These 
impulsive  changes/  if  used/  usually  emulate  a  feedback  cor¬ 
rection  path  not  directly  affected  by  (2-32).  A  pure  error 
state  formulation  of  the  filter  can  give  rise  to  the  need 
for  such  impulsive  changes.  Another  control  option  is  con¬ 
tinuous  control  which  was  discussed  earlier  in  Subsection 
2.1.  Control  options  involving  combinations  of  continuous 
control  and  discrete  resets  may  also  be  implemented.  The 
fact  is  that  the  method  of  control  depends  on  linearity  or 
lack  of  it/  on  full  state  or  error  state  formulation,  on  ac¬ 
cessibility  of  feedback  paths,  etc.,  and  is  highly 
problem-dependent.  It  should  be  possible  to  Implement  most 
forms  of  control  with  the  structures  already  available  in 
SOFE. 


2.8  Vector  St  ructur e 

To  avoid  the  overhead  associated  with  double  subscript¬ 
ing,  both  Pf  and  S  are  carried  in  SOFE  as  vectors  (linear 
arrays).  Since  Pf  is  symmetric  and  S  is  upper  triangular, 
only  the  upper  triangular  part  of  each  matrix  need  be  saved 
to  preserve  Its  information.  The  scheme  for  constructing 
the  appropriate  vector  from  its  matrix  is  to  scan  the  upper 
triangular  part  of  each  matrix  columnwise  starting  with  the 
1-1  element: 


33 


(2-36) 


Pf-vector  =  (pll  p12  p22  p13  p23  p33  p14  ...)T 
S-vector  =  (sll  s12  s22  s13  s23  s33  sU  ...)T  (2-37) 


The  size  of  these  vectors  is 


NTR  =  NF  (NF  +  1  )  /  2 


(2-38) 


For  propagation,  the  three  vectors  of  interest  are  its, 
Xf  and  Pf-vector,  which  are  concatenated  into  a  composite 
named  Y. 


Y 


A 


Xs 

Xf 

Pf-vector 


(2-39) 


•  • 
The  derivative  _Y  is  governed  by  equation  (2-1)  for  Xs, 

•  • 

(2-27)  for  Xf,  and  (2-28)  for  Pf.  The  user  supplies  £(•> 
for  Xs  in  XSDOT,  Xf  in  XFDOT,  and  F  and  Qf  for  Pf  in  FQGEN. 
Of  course,  Pf  could  not  be  implemented  using  the  straight¬ 
forward  matrix  adds  and  multiplies  of  (2-28)  because  of  the 

vector  storage  mode  of  Pf.  Two  special  sparse  matrix  rou- 

• 

tines  were  written  to  form  a  Pf-vector  equivalent  to  (2-28). 


a 

In  update,  the  vectors  of  Interest  are  Xf  and  S-vector. 
in  Appendix  C  of  C33,  Carlson  gives  computer  algorithms  for 
update  of  S-vector  carried  as  prescribed  in  (2-37).  His  al- 
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gorithms  were  used  as  written. 


2.9  Summa  ry 

This  section  has  developed  equations  for  propagation  of 
a  truth  model  state  £s,  and  for  propagation  and  update  of  a 
filter  model  composed  of  a  state  X.f  and  an  error  covariance 
pf.  All  propagation  is  accomplished  in  SOFE  via  numerical 
integration  using  a  se  l  f -s t a r 1 1 ng ,  fifth-order,  Runge-Kutta 
type  differential  equation  solver  having  automatic  error 
control  via  step-si2e  adjustment.  Update  is  accomplished 
using  the  algebraic  relationships  in  (2-29)  through  (2-32). 
The  operative  equations  for  the  truth  model  are  given  in 
Section  2.1  and  are  summarized  for  the  filter  model  in  Fig¬ 
ure  2-2. 
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3.0  PROGRAM  DESCRIPTION 

This  section  presents  Information  about  the  concept, 
structure  and  coding  conventions  of  SOFE.  Our  goal  Is  to 
convey  the  approach  and  the  Implementation  that  were  devel¬ 
oped  for  solving  the  s imut at  1 on/Ka Iman  estimation  problem 
outlined  in  Sections  1  and  2. 

3 . 1  Program  Concept 

SOFE  is  Intended  as  an  efficient  general-purpose  tool 
for  expediting  the  construction  of  a  simulation  for  Kalman 
filter  design  and  system  performance  analysis.  As  such.  It 
carries  the  flexibility  to  handle  a  wide  variety  of  problems 
without  revision  In  Its  basic  structure.  We  now  discuss 
some  of  the  strategies  that  were  used  to  achieve  the  afore¬ 
mentioned  goals. 

Examination  of  filter  design  simulations  shows  that 

tasks  performed  may  be  grouped  Into  these  eight  categories: 

o  Data  1/0 
o  Problem  setup 
o  Run  Initialization 
o  Time  propagation 
o  Measurement  update 
o  Feedback  correction 
o  Run  termination 
o  Problem  termination 

These  eight  tasks  are  organized  Into  the  macro-level  flow 
chart  In  Figure  3-1. 
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Figura  3-1.  Macro  Laval  Flo*  Chari. 


Any  filter  design  simulation  must  be  able  to  carry  out  these 
tasks  and  SOFE  is  no  different  in  this  regard.  SOFE  differs 
in  that  more  of  the  data  for  accomplishing  these  tasks  can 
be  user-supplied. 

The  aforementioned  data  are  of  two  types,  constant  and 
variable.  The  constant  data,  which  are  read  into  SOFE  as 
card  input*  (Subsection  A. 1.1),  do  not  change  as  the  simula¬ 
tion  evolves  in  time.  These  data  contain  items  such  as  the 
dimensions  of  X.s  and  tf,  the  time  intervals  between  various 
events,  I/O  control  parameters,  etc.  These  constant  data 
must  be  supplied  to  any  digital  simulation. 

The  variable  data  are  generated  by  user-written  rou¬ 
tines  that  are  called  periodically  to  compute  time-varying 
quantities  that  bear  on  the  problem  solution.  Examples  of 
such  quantities  are  derivatives,  measurements,  truth  model 
fluctuations,  trajectory  data,  etc.  These  user-written  rou¬ 
tines  give  SOFE  the  flexibility  to  handle  most  Kalman  filter 
design  studies. 

Although  user-written  routines  do  provide  flexibility, 
they  represent  extra  time  and  work  to  gain  access  to  SOFE. 
Our  constant  goal  was  to  keep  the  user's  work  to  a  minimum 
by  doing  as  much  as  possible  of  the  repetitious  portion  of 
the  problem  in  basic  SOFE.  This  'principle'  produced  these 
interfaces  between  user  code  and  basic  SOFE  code. 
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o  Tine  propagation  Is  aecoapllshed  using  a  (fifth 
order)  numerical  Integrator  In  basic  SOFE. 
Derivative  values  are  supplied  In  user-written  su¬ 
broutines  XSDOT,  X FOOT  and  F8GE N . 

o  Basic  SOFE  propagates  the  homogeneous  part  of 
dj(s/dt  while  the  user  oust  Inject  random  noise  as  an 
Impulse  change  to  £s  using  user-written  subroutine 
SNOYS. 

o  All  update  processing  of  ti  and  Pf  occurs  In  basic 
SOFE  but  the  user  aust  supply  H,  Rf  and  Z  residual 
In  HRZ. 

o  The  user  applies  whatever  lapulsive  control  he 
wishes  using  subroutine  AMEND.  (Any  continuous  con¬ 
trol  would  be  specified  to  basic  SOFE  through  the 
derivatives  In  XSDOT  and  XFDOT.) 

o  Basic  SOFE  does  I/O  but  USRIN  and  ESTIX  are  called 
froa  basic  SOFE  for  user-specific  Input  and  output 
respect  1 ve  ly . 

o  Basic  SOFE  will  read  and  Interpolate  trajectory 
data  but.  If  he  so  desires,  the  user  can  construct 
his  own  trajectory  during  execution  using 
user-written  TRAJ. 


A  aore  detailed  picture  of  the  Interaction  of  user-written 
and  basic  routines  Is  found  In  Subsection  3.Z 


SOFE  Is  designed  to  be  efficient  In  two  areas:  use  of 
core  and  tlae.  Core  savings  are  obtained  by  the  following 
aeans: 


o  Since  Pf  Is  syaaetrlc,  coaplete  covariance  Infor¬ 
mation  Is  retained  when  only  the  upper  triangular 
portion  (plj  ,  1<j>  Is  processed.  SOFE  propagation, 
update  and  I/O  algorithms  are  designed  for  this 
upper  triangular  storage  mode.  The  collected  sav¬ 
ings  In  Pf  and  S  total  NF(NF-I)  words  of  core. 


/ 
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o  Since  F  and  Of  are  often  sparse  matrices,  space  is 
provided  only  for  their  nonzero  values.  This  re¬ 
quires  storing  two  additional  words  for  the 
row-column  indices  of  each  nonzero  element,  but  usu¬ 
ally  there  is  a  substantial  net  saving  of  core. 

o  All  vectors  and  matrices  needed  to  solve  the 
user's  problem  are  retained  in  unlabeled  COMMON  area 
A  in  a  dense  format.  When  SOFE  needs  a  particular 
array,  that  array  is  found  from  its  first-word  ad¬ 
dress  in  A,  an  address  that  is  assigned  at  problem 
setup  based  on  dimensions  and  sizes  specified  by  the 
user.  Putting  all  arrays  in  A  allows  the  user  to 
shrink  or  enlarge  A  to  fit  his  problem,  a  change 
that  can  be  made  by  altering  just  two  statements  in 
the  main  routine  (see  comments  in  SOFE). 

o  The  general  working  space  is  only  NF  +  2M  words,  a 
relatively  small  number. 


Computing  time  savings  are  obtained  by  these  means: 


o  Sparse  matrix  manipulation  methods  are  used  to 
form  the  derivative  of  Pf.  Subroutines  FPPPFT  and 
ASYSP  exploit  the  sparse  nature  of  F  and  Qf  by  elim¬ 
inating  all  zero  multiplies. 

o  Elimination  of  most  doubly  subscripted  arrays  in 
favor  of  singly  subscripted  vectors. 


Reference  1  contains  further  details  about  most  of  these  ef¬ 
ficiency  techniques.  Note  that  additional  computational 
savings  were  obtained  In  SOFE  by  switching  to  an  upper  tri¬ 
angular  storage  node  for  Pf.  The  Indexing  computations  are 
remarkably  simpler  than  those  for  the  lower  storage  mode 
used  In  C13  and  C23. 
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3 • 2  PrpQr am  Structure 


SOFE  Is  a  nodular  computer  program  consisting  of  a  main 
executive,  29  subprograms  and  a  block  data  routine.  SOFE 
was  constructed  using  a  'top  down'  approach.  Thus  it  con¬ 
tains  a  small  number  of  top  level,  mainly  logical  routines 
to  provide  sequencing  and  control  while  the  computational 
algorithms  are  relegated  to  lower  level  routines.  This 
structure  is  visible  in  Figure  3-2,  a  subroutine  dependency 
chart  showing,  in  approximate  time  order,  what  calls  what. 
The  reader  will  note  the  correspondence  of  the  descriptive 
phrases  on  the  right  of  Figure  3-2  and  the  operations  in 
F 1 gur e  3-1  . 

A  complete  review  of  program  structure  would  require 
discussion  of  individual  subroutines.  Such  a  discussion  is 
beyond  the  scope  of  this  user's  manual,  but  a  few  notes 
about  the  executive  structure  are  needed.  Two  routines  con¬ 
tain  almost  all  the  executive  functions:  SOFE  and  ADVANS. 

SOFE  is  the  main  executive.  It  controls  problem  and 
run  initialization,  measurement  update  process i ng,  and  feed¬ 
back.  ADVANS  is  In  charge  of  propagation  via  numerical  in-- 
tegration.  It  schedules  all  periodic  events  and  forces  the 
Integration  to  pause  at  each  event  time  so  the  event  action 
may  take  place.  The  six  events  are: 
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•OTt  f«kpr*fr«a  (4  ,f  4> 


o  Update  o  Printer  plot  output 

o  Printed  output  o  User  output 

o  Calcomp  output  o  Noise  injection  in  X^s 

ADVANS  also  causes  the  correct  data  to  be  selected  from  the 
external  trajectory  tape  (TAPE3)  for  integration  and  update 
purposes. 

SOFE  uses  ten  labeled  COMMON  areas  to  store  data  perti¬ 
nent  to  its  internal  workings.  Table  3-1  lists  these  areas 
and  their  contents. 


Table  3-1 

LABELED  COMMON  DESCRIPTIONS 


Area 

Contents 

DAYTIM 

Simulation  date  and  time 

DELTAT 

Event  time  intervals 

DIFEQ 

Integration  related  quantities 

I  COM 

A  mixture  of  control  and  Index 
data  beginning  with  letter  I 

IPOINT 

First  word  address  of  all  arrays 
In  unlabeled  COMMON  area  A 

LCOM 

Logical  parameters  for  output 
control 

N  COM 

Dimensions  and  sizes 

OTHER 

Measurement  residual  and  exter¬ 
nal  trajectory  control 

T  COM 

Times  given  at  input 

TITLE 

Problem  title 

The  user  must  not  name  any  COMMON  area  in  his  code  with  one 
of  the  above  ten  names  nor  should  he  attempt  to  use  any  un¬ 
labeled  COMMON. 
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3.3  Codl nq  Convent  1 on» 

SOFE  Is  written  in  FORTRAN  using  Mainly  1966  ANSI  stan¬ 
dard  constructs.  Exceptions  to  the  ANSI  standard  rule  are: 

o  Use  of  the  'NAMELIST'  and  'list  directed*  conven¬ 
tions  for  reading  input 

o  In  FORMAT  statements,  use  of  special  syMbols  for 
tabbing  and  dellMlting  Hollerith  data 

o  Use  of  the  ENTRY  statement 

o  Use  of  comments  following  STOPs 

o  DATA  statements  for  arrays 

o  Use  of  the  ENCODE  capability  (in  GOPLOT  only) 
o  Use  of  the  octal  constant  (in  GOPLOT  only) 

Insofar  as  possible,  routines  have  been  kept  to  a  sin¬ 
gle  page  for  readability  purposes.  Each  routine  has  a  set 
of  comments  at  Its  beginning  describing  Its  function. 
Comments  are  sprinkled  throughout  the  code  and  considerable 
effort  has  been  made  to  make  them  complete.  Informative  and 
accurate.  The  following  order  was  used  to  list  the  nonexe¬ 
cutable  statements  at  the  beginning  of  each  routine: 

0  COMMON 
o  DIMENSION 
o  EQUIVALENCE 
0  EXTERNAL 
o  Type 
o  DATA 
O  NAMELIST 

Note  that  equivalence  statements  were  rarely  used.  Also 
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note  that  only  logical  variables  were  'typed'  since  the 
first  character  default  rule  for  real  and  integer  variables 
was  followed  throughout.  Required  format  statements  appear 
following  the  last  return  statement. 

SO  FE  uses  only  single  precision  variables  of  the  REAL, 
INTE6ER  and  LOGICAL  type.  Variables  are  given  meaningful 
names  from  the  36  alphanumeric  characters  (no  special  sym¬ 
bols). 
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4.0  SOFE  INTERFACES 


This  section  covers  input,  output  and  the  construction 
of  user-written  routines.  It  begins  with  an  overview  that 
illustrates  the  flow  of  input  (data  and  program  modules)  to 
the  computer  and  the  output  of  information  from  the  com¬ 
puter  . 


All  input  and  output  in  SOFE  is  accomplished  through 
external  data  files  called  tapes.  It  is  usually  most  con¬ 
venient  for  all  of  these  files  to  reside  on  disk  or  magnetic 
tape,  although  one  file,  TAPE5,  may  be  input  from  cards. 
SOFE  also  accepts  input  from  up  to  two  special  files,  TAPEs 
3  and  9.  SOFE  generates  listable  output  on  TAPE6,  output 
for  Calcomp  plots  on  TAPE4  and  output  for  problem  continua¬ 
tion  purposes  on  TAPE10.  Also  provided  are  TAPE8  for 
user-defined  output  and  TAPE7  for  accumulation  of  printer 
plot  data.  These  allocations  are  summarized  in  Table  4-1. 


Table  4-1 


SOFE  FILE  DEFINITION 


TAPE3 

Input 

External  trajectory  data 

TAPE4 

Ouput 

Data  for  Calcomp  plots 

TAPES 

Input 

Cards  defining  user's  problem 

TAPE6 

Output 

Listable  output  device 

TAPE7 

Temporary 

Data  for  printer  plots 

TAPE8 

Output 

User  defined 

TAPE9 

Input 

Initial  values  of  X^s,Xl,9l 

TAPE10 

Output 

Final  values  of  X$,Xf~Pf 
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These  tapes  are  sized  and  ordered  on  the  FORTRAN  program 
card  in  the  main  routine  of  SOFE  as  follows: 

PROGRAM  SO  FE (T AP E 5- 64 / 80 , T APE3 , T APE9* 5 1 2 , 

0UTPUT,TAPE4,TAPE6=0UTPUT,TAPE8=512,TAPE10=512, 
TAPE7=51 2) 

Note  the  explicit  naming  of  OUTPUT ,  the  listable  output 
device  or  printer,  the  absence  of  INPUT,  and  the  position  of 
TAPE5.  These  factors  can  affect  job  control  which  is  dis¬ 
cussed  more  fully  in  Appendix  D. 

Figure  4-1  shows  the  flow  of  SOFE  and  its  data  to  and 
from  the  computer.  The  solid  (dashed  I  lines  that  connect  to 
the  computer  indicate  which  tapes  are  mandatory  (optional). 
The  flow  shown  in  Figure  4-1  was  devised  for  a  CDC  computer 
but  would  be  essentially  the  same  on  any  computer.  Note  the 
special  input  module  called  'user-written  routines'.  This 
module  contains  nine  routines  that  together  with  the  31  rou¬ 
tines  of  basic  SOFE  produce  a  complete  program. 

4.1  Input 

Table  4-1  shows  SOFE  input  on  TAPES  3,  5  and  9.  TAPES 
3  and  9  are  potentially  large  files  that  will  usually  re¬ 
quire  magnetic  tape  or  disk  storage.  TAPES  is  a  small  file 
that  can  be  constructed  on  cards,  if  desired.  TAPEs  3  and  9 
are  rewound  in  subroutine  SOFEIN  during  SOFE  Initialization. 


TAPES  is  not  rewound 


4.1.1  T  APE  5  (Card)  I nput ,  Prob  Lew  Setup  and  Control 

TAPES  input,  which  we  will  often  refer  to  as  card 
input.  Is  read  with  FORTRAN  read  statements  like  the  follow¬ 
ing  three: 

A.  READ  (5,100)  TITLE 

B.  READ  (5,*)  IRON, ICOL, PDUM 

C.  READ  (5, PR  DA  TA ) 

Type  A  Is  the  familiar  formatted  read.  SOFE  uses  it  only 
for  input  of  'alpha'  data  in  A  format.  Type  B  is  a  special 
free-form  convention  that  CDC  calls  'list  directed'.  Data 
input  under  this  convention  must  be  in  order  but  need  not 
reside  in  preassigned  columns  on  the  card.  In  addition, 
multiple  quantities  may  be  entered  on  a  single  card  separat¬ 
ed  only  by  commas.  Type  C  is  a  special  convention  called 
NAMELIST  which  is  also  of  the  free-form  variety.  Taken  to¬ 
gether,  the  NAMELIST  and  list  directed  conventions  form  a 
complete  free-form  capability.  The  user  should  consult  the 
CDC  FORTRAN  manual  for  complete  details  on  these  conven¬ 
tions.  Examples  of  both  are  presented  herein.  Warning  to 
CDC  users:  avoid  the  TS  and  EDITOR  compilers  because  they 
occasionally  choke  on  list  directed  input. 

Table  4-2  Is  the  ordered  list  of  the  quantities  to  be 
read  from  cards,  and  Figure  4-2  is  an  example  set  of  card 
input  for  the  INS  problem  discussed  in  Section  5  and  Appen- 
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dix  A.  Blank  lines  are  inserted  between  some  data  sets  in 
Figure  4-2  to  enhance  readability.  They  are  not  required, 
but  they  do  no  harm  when  used  as  shown. 

The  title,  in  20A4  format,  is  the  first  card.  It  must 
be  present  and  may  occupy  up  to  one  full  card  (80  columns). 
The  PRDATA  namelist,  containing  problem  definition  parame¬ 
ters,  I/O  control  flags  and  integration  specifications,  fol¬ 
lows.  If  the  user  desired  only  defaults,  he  would  enter 
SPRDATAS.  The  next  section  offers  a  full  discussion  of  each 
PRDATA  parameter.  Both  the  title  and  the  PRDATA  list  are 
read  from  subroutine  SOFEIN. 

The  next  card(s)  contains  the  nonzero  indices  for  F. 
Each  index  pair  is  the  row-column  location  of  a  nonzero  ele¬ 
ment  in  F.  These  pairs  may  be  entered  in  any  order  so  long 
as  the  order  chosen  agrees  with  that  used  for  nonzero  F(i) 
•valuation  in  FQGE N  (see  4.3.3).  The  nonzero  indices  of  Qf 
follow,  with  the  same  order  convention  as  F.  Since  Qf  is 
symmetric,  only  the  indices  of  nonzero  elements  on  and  above 
the  diagonal  need  be  entered.  All  nonzero  indices  for  both 
F  and  Qf  are  read  from  NZRCIO. 
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Table  4-2 


SOFE  CARO  INPUT  SEQUENCE 


I  ten 

Number 

Format 

Optional 

Problem  title 

1 

2(574 

No 

PROATA  group 

1 

Name l i st 

No 

(1) 

Row-column  indices  for 

NZF 

List  Directed 

No 

(2) 

nonzero  elements  of  f 

Row-column  indices  for 

NZQ 

List  Di rected 

No 

(3) 

nonzero  elements  of  Of 

Initial  values  Xso 

NS 

List  Directed 

No 

(4) 

initial  values  Xfo 

NF 

List  Directed 

No 

(4) 

row  index , column  index 
and  value  of  all  non- 

<NF**2 

List  Directed 

No 

(4) 

zero  elements  of  Pfo. 

A  0,0,0.  card  terminates 
this  input. 

User  defined  input  as 

- 

- 

Yes 

called  from  USRIN 

Time-axis  scale  factor 

1 

List  Directed 

Yes 

(5) 

Plot  parameter  sets 

A  0,0, 0,0.  card 

<20 

List  Directed 

Yes 

(5 ) 

terminates  this  input. 

Time-axis  title 

1 

3  A1Q 

Yes 

( 5  > 

Title  for  itj>  plot 

<20 

8A10 

Yes 

(5) 

Y-axis  title  for 

<20 

3A10 

Yes 

(5) 

1 th  plot  <6) 


(1) 

At  least  SPRDATAS  must 

be 

gi ven. 

even 

if  only  defaults  are 

desired. 

(2) 

These 

data 

are 

omitted 

if 

NZF  *  0. 

(3) 

These 

data 

are 

omitted 

if 

NZQ  *  0. 

(4) 

These 

data 

are 

omitted 

if 

IC0NT  »  1. 

(5) 

These 

data 

are 

requi red 

only  if  LPP  is  TRUE. 

(6) 

Repeat  the 

last 

two  items 

once  for  each  plot 
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colunn  1 

GCAP/MCAP  SINGLE  AXIS  INS  -  A  STANDARD  LONG  TEST  FOR 

SPRDATA 

NF*5,  N$*9,  N*2,  NZ F=7,  NZ  9  =2  ,  NXTJ=1,  TF=3600Q., 

DTNE  A  S  =3  0. ,  DT  PR  NT  =3  600  .  ,  DT  PR  PL  =3  60./  0TN0YS=30., 
LPP=.T.,IPGSIZ*55,  * 

1*2,  2*3,  2,5,  3,2,  3,4,  4,4,  5,5 


4,4,  5,5 


9*0 . 

5*0. 

1,1,14400. 

2/2,4. 

3,3,3. 046E -6 
4,4,2. 350E  -1  5 
5,5,4. 147E -5 
0,0,0. 


SINF  T All F  C  1  )  =  3  600 .  ,  300., 

SDWF<1  )  =  4.848E-8,  6.442E-3, 

RFVCTR (1 )=1 0000.0,  0.25,  S 

$1  NS  TAUS  <1  )=3600. ,  300.,  3600.,  1800.0,  300.0, 

SDNS  (1  )  =  4.848E-8,  6.442E-3,  3.22E-3,  3.0E+2,  5.0E-1, 
SDNS  0=2 . 42E -8,  S 


1.0 


1,1, 1/1. 

2/ 2, 2, 1 . 

3,3,3,3437.75 

4,4,4,206265. 

5/5, 5,1. 

0, 0,0,0. 

TINE  (SECONDS) 

PLOT  PARAMETER  SET  1,1,1 
POSITION  *FEET* 

PLOT  PARAMETER  SET  2,2,2 
VELOCITY  *FEET  PER  SECOND* 

PLOT  PARAMETER  SET  3,3,3 
TILT  *ARC  MINUTES* 

PLOT  PARAMETER  SET  4,4,4 
GYRO  DRIFT  *DE 6  PER  HR* 

PLOT  PARAMETER  SET  5,5,5 
ACCEL  8IAS  *  FEET  PER  SEC0ND2* 


FIGURE  4-2.  SAMPLE  OF  CARD  INPUT 


*  S  OF  E  ' 
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Initial  conditions  for  X^s  and  XI,  denoted  £so  and  Xfo, 
are  entered  next.  There  lust  be  NS  entries  for  Xso  and  NF 
for  xio.  In  the  example,  repetition  factors  of  o  (9*0.)  and 
5  (5*0.)  have  been  used  to  expedite  this  input. 

The  nonzero  values  of  the  initial  covariance  Pfo  are 
entered  next.  These  values  are  entered  in  any  order  by  giv¬ 
ing  their  row-column  location  followed  by  their  numeric 
value.  In  the  example.  Figure  4-2,  only  one  entry  per  line 
has  been  used,  but  multiple  entries  are  possible  if  each 
pair  is  separated  by  a  slash  (see  Figure  B-2  for  an  exam¬ 
ple).  Since  Pf  is  symmetric,  only  nonzero  values  on  and 
above  the  diagonal  need  be  specified.  Should  values  below 
the  diagonal  be  found,  their  row-column  indices  are  inter¬ 
changed  before  normal  storage  occurs.  A  card  having  a  non¬ 
positive  entry  in  either  the  row  or  column  position  signals 
the  end  of  Pfo  input. 

Xso  and  Xio  are  read  from  GETX,  while  Pfo  is  read  from 
GETPF.  If  ICONT  is  1,  initial  state  and  covariance  data  are 
obtained  from  TAPE9  and  should  be  omitted  from  cards  (see 
4.1  .1  .1)  , 

USRIN  input  is  entered  next.  It  can  have  any  FORTRAN 
input  form,  the  only  requirement  being  that  it  reside  at 
this  location  in  the  card  deck. 
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The  last  set  of  data  to  be  entered  trow  cards  is  that 


governing  printer  plotting.  Several  printer  plot  options 
are  available  as  discussed  in  4. 1.1. 2.  The  time-axis  scale 
factor  is  given  first,  followed  by  a  max  of  20  plot  parame¬ 
ter  sets,  one  per  card.  A  card  whose  first  three  entries 
are  nonpositive  signals  the  end  of  the  plot  parameter  sets. 
These  plot  data  are  read  during  SOFE  initialization  by  PLPR. 
At  SOFE  conclusion  PLPR  reads  the  time-axis  title  followed 
by  the  plot  title  and  V-axis  title  for  each  plot.  Note  the 
A  formats  for  these  titles  in  Table  4-2. 

4. 1.1.1  PR  DA  T  A  NAMELIST  Definitions 

Forty  parameters  are  entered  through  the  PR  DA  TA  list  in 
CDC  NAMELIST  format.  These  parameters,  which  remain  fixed 
throughout  the  simulation,  specify  the  user’s  problem,  con¬ 
trol  I/O,  and  regulate  numerical  integration.  All  parame¬ 
ters  have  a  default  value  (see  Table  4-3)  that  is  invoked  in 
lieu  of  input  data.  All  numeric  parameters  are  single  pre¬ 
cision.  The  following  list  defines  each  parameter  and  gives 
explanatory  data  as  required.  If  the  parameter  is  logical, 
the  definition  given  is  for  its  true  state. 

PARAMETER  Type  un i t s 

NF  Integer 

The  number  of  states  in  the  filter  model. 
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NS 


n 


NZF 


NZQ 


NXT  J 


LXTJ 


TO 


(1)  Second*  are 
cretenes*.  If 
Minute*/  hour*/ 
SOFE. 


Integer 

The  number  of  states  in  the  truth  model. 


Integer 

The  number  of  measurements  to  be  processed 
at  update  time.  If  M  is  0/  updates  are  not 
attempted . 


Integer 

The  number  of  non-zero  elements  in  the  NFxNF 
filter  dynamics  partial  matrix  F. 

Integer 

The  number  of  non-zero  elements  in  the  NFxNF 
filter  noise  strength  matrix  Of. 


Integer 

The  number  of  variables/  not  counting  time, 
to  be  read  from  the  external  'trajectory' 
data  tape  (TAPE3). 


Logical 

If  TRUE/  'trajectory'  data  is  external/ 
i.e./  available  to  SOFE  on  T  A  PE  3 .  Each 
TAPE3  record  is  binary  and  consists  of  time 
followed  by  NXTJ  variables.  A  description 
of  TAPE3  structure  is  given  in  Subsection 
4.1.2.  If  FALSE/  any  trajectory  data  re¬ 
quired  to  find  F/  H/  Xs,  etc./  must  be 

generated  during  the  simulation  in 
user-supplied  routines/  e.g.  in  TRAJ. 


Real  seconds  (1) 

Initial  time  of  each  run.  Note:  the  last 
character  in  parameter  names  TO,  THEASO  and 
HO  is  a  zero. 


shown  here  and  elsewhere  primarily  for  con- 
the  user  wishes  to  scale  his  problem  In 
day*/  etc./  he  may  do  so  without  altering 
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TF 


Real 


seconds 


TMEASO 


DTMEAS 


DT  PR  NT 


DTCCPL 


OT  PR  PL 


DTSTIX 


Final  time  of  each  run.  The  simulation  will 
not  run  backwards,  so  TF  must  be  greater 
than  TO. 


Real  seconds 

Updates  are  prohibited  before  this  time. 


Real  seconds 

The  time  Interval  between  measurements. 
DTMEAS  and  the  next  five  DT....  quantities 
are  referenced  to  zero  seconds;  e.g.,  if  TO 
*  50.  and  DTPRNT  *  6.,  the  simulation  will 
pause  to  print  output  at  T  *  150.,  54.,  60., 
66.,...). 


Real  seconds 

Print  Interval.  See  LPR . 


Real 

Data  storage  interval 
See  LCC, 


seconds 

for  Calcomp  plots 


Real  seconds 

Data  storage  Interval  for  printer  plots. 
Sampling  will  occur  at  a  max  of  only  101 
times.  More  samples  are  unwarranted  because 
of  limited  printer  plot  resolution.  See 
IPP. 


Real  seconds 

The  time  Interval  between  calls  to 
user-subroutine  ESTIX.  The  user  may  wish  to 
use  these  calls  to  sample  error,  to  con- 
struct  some  statistic,  to  output  something 
not  controlled  by  a  PtDATA  parameter,  to 
modify  a  data  base,  etc.  TAPES  is  provided 
for  output  from  ESTIX. 
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DTNOYS 


Real 


seconds 


L  PR 


LPRXS 


LPRXF 


LPRDG 


LPRLT 


LPRUD 


The  time  interval  between  calls  to 
user-sub  rout i ne  SNOYS.  These  calls  are  for 
the  purpose  of  adding  noise  to  the  truth 
state. 


Logical 

Master  control  for  printing.  If  TRUE,  for¬ 
matted  writes  are  made  to  TAPE  6  at  synchro¬ 
nous  intervals  governed  by  DTPRNT.  All 
other  parameters  that  begin  with  the  letters 
'  L  PR  1  also  control  printing  and  are  logical¬ 
ly  'ANDED*  with  L  PR .  TAPE  6  is  for  all  list- 
able  output  and  will  contain  error  messages, 
summary  data,  printer  plots,  etc.,  in  addi¬ 
tion  to  the  synchronous  output  governed  by 
LPR .  See  I  PR  RUN . 


Logical 

Prints  truth  state  vector  Xs  at  the  interval 
specified  by  DTPRNT. 


Logi cal 

Prints  filter  state  vector  Xf  at  DTPRNT  in¬ 
terval. 


Logical 

Prints  the  square  root  of  the  diagonal  ele¬ 
ments  of  Pf  at  DTPRNT  interval.  Thesg  are 
the  one-sigma  values  for  the  states  in  Xf . 


Logi cal 

Prints  the  symmetric  covariance  matrix  Pf  in 
a  lower  triangular  display  at  DTPRNT  inter¬ 
val  . 


Logical 

Allows  printed  output  of  states  and  covari¬ 
ance  values  at  update  time.  The  previous 
five  parameters  still  govern  what  is  print¬ 
ed  . 


I  PR  ZR 


Logical 


L  PR  H 


LPRK 


LPRXTJ 


LCC 


LPP 


LPPLO 


Prints  measurement  residuals  and  their  stan¬ 
dard  deviations  a t_  update  time.  These  are 
the  residuals  ZRES=0Zj  constructed  by  the 
user  in  subroutine  HRZ.  The  residual  stan¬ 
dard  deviation  is  the  square  root  of 
Rf+H*Pf 


Logi cal 

Prints  the  vector  H  at  update  time. 


Logical 

Prints  the  Kalman  gain  K  at  update  time. 


Logi cat 

Prints  interpolated  external  trajectory  data 
at  DTPRNT  intervals  if  LXT J  is  true. 


Log i c  a l 

Master  control  for  Calcomp  plotting.  If 
TRUE,  LCC  enables  storage  of  data  on  T  APE  4 
(at  TO,  DTCCPL  intervals,  update  times  and 
at  TF)  for  subsequent  Calcomp  plotting  by 
SOFEPL,  C53. 


Logical 

Master  control  for  printer  plotting.  If 
TRUE,  LPP  enables  storage  of  data  on  TAPE  7 
(at  TO,  DTPRPL  intervals,  update  times  if 
LPPUP  is  true,  and  at  TF)  during  the  first 
run  for  generation  of  printer  plots  at  prob¬ 
lem  completion.  Up  to  20  plots  may  be  made, 
each  containing  a  max  of  101  time  samples. 
Control  of  what  gets  plotted  is  through  data 
cards  described  below. 


Logical 

Lists  the  data  to  be  plotted  when  printer 
plots  are  generated.  Each  (x,y)  pair  for 
each  curve  is  listed. 
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LPPUP 


Log  1 cal 


1C0NT 


ISEED 


IPASS 


I  PR  RUN 


IPGSIZ 


All ows  output  to  the  printer  plot  file 
CTAPE7)  at  update  tines  If  printer  plots  are 
generated.  The  counterpart  for  printed  out¬ 
put  Is  L  PR  U  B . 


Integer 

A 

Control  for  Xso,  Xfo,  and  Pfo  Input.  If  set 
to  1,  this  problem  Is  the  continuation  of  a 
previous  problem  and  the  above  values  are 
read  from  T  A  PE  9 .  If  other  than  1 ,  these 
values  are  read  fro*  TAPE  5  (new  run). 


Integer 

Seed  value  for  random  number  generator. 
ISEEO  Is  used  to  Initialize  the  random 
number  generator  and  thereby  assure  that  a 
random  nunber  sequence  can  be  repeated. 


Integer  runs 

Nunber  of  runs  over  [T0,TF3  In  the  Honte 
Carlo  simulation. 


Integer  run s 

Nunber  of  runs  for  which  printed  output  Is 
desired.  IPRRUN  has  no  effect  If  LPR  Is 
false.  If  LPR  is  true,  all  synchronous 
printed  output  Is  disabled  after  run  IPRRUN 
Is  complete. 


Integer  lines 

The  nunber  of  lines  to  be  printed  on  each 
page.  On  most  printers,  approximately  60 
lines  fill  one  page.  If  more  are  printed, 
some  will  fall  on  the  fold  between  pages  un¬ 
less  the  printer  has  its  own  page  size  con¬ 
trol.  If  ZP6SIZ  Is  not  positive,  page  con¬ 
trol  Is  turned  off  in  SOFE. 
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MODE  Integer 

Indicates  type  of  integration.  If  1,  the 
step  size  is  'variable',  i.e.,  H  is  ad¬ 
justed  automatically  to  Maintain  the  inte¬ 
gration  error  below  its  allowed  value.  If 
not  1,  the  step  size  is  fixed  at  HO. 
Variable  step  integration  is  strongly  recom- 
■ended . 


TOLER  Real 

The  peraissible  integration  error  per  unit 
step  when  variable  step  mode  is  used.  This 
tolerance,  which  applies  equally  to  all  var¬ 
iables,  is  applied  as  a  relative  (absolute) 
criterion  when  a  variable's  magnitude  is  >1 
«1  ). 

HM  AX  Real  seconds 

Maximum  permissible  step  size.  This  quan¬ 
tity  is  intended  to  be  a  measure  of  the 
'scale'  of  the  problem.  If  the  integrator 
never  uses  a  step  size  larger  than  HM  AX,  it 
should  never  step  over,  and  therefore  miss 
completely,  any  fluctuations  in  the  solu¬ 
tion.  In  effect,  HM  AX  tells  the  integrator 
approximately  how  fine  a  mesh  is  needed  for 
a  reasonable  attempt  at  solving  the  numeri¬ 
cal  integration  problem. 


HM IN  Real  seconds 

Minimum  permissible  step  size.  If,  in  or¬ 
der  to  handle  severe  dynamics,  the  integra¬ 
tor  reduces  its  step  size  to  HMIN  without 
satisfying  the  specified  error  criterion,  an 
integration  failure  has  occurred.  If  this 
happens,  an  error  message  is  printed  and  the 
simulation  stops. 


HO  Real  seconds 

Initial  Integration  step  size.  See  MODE. 
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/ 


A  word  of  explanation  and  caution  is  needed  here.  SOFE 
performs  events  in  the  order  prescribed  by  the  six  DTs. 
When  events  occur  simultaneously,  SOFE  does  them  in  reverse 
order  to  the  DT  list  above:  i.e.  calls  to  SNOYS  first, 
calls  to  EST1X  second,  ...  ,  measurement  processing  last. 

To  illustrate,  if  both  a  print  and  a  measurement  were  sche¬ 
duled  at  T=50,  the  print  would  be  done  first.  Mow  suppose 
DTPRNT*3 .4,  DTHEAS=10.2  and  T  is  approaching  30.6.  This  ap¬ 
pears  to  be  a  simultaneous  event  situation,  but  this  time 
the  measurement  will  be  done  first  because  its  event  time 
computes  to  30.599...  while  the  prints  event  time  computes 
to  30.600...  .  A  computed  event  time  can  be  slightly  in 

error  (either  low  or  high)  whenever  its  DT  cannot  be  exactly 
represented  in  a  full  computer  word,  i.e.  is  not  a  power  of 
two.  For  example,  neither  3.4  nor  10.2  are  so  representable 
but  3.375  and  10.25  are.  If  the  user  wants 
random  ordering  of  events  that  can  occur  when  T 
multiple  of  several  DTs,  he  must  choose  each  DT 
ly  representable  in  one  computer  word. 


to  avoid  the 

is  a  common 

to  be  exact-  « 
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Table  4-3 


PR  DA  TA  RANGE  AND  DEFAULTS 


Par  awe  ter 

NF 

NS 

M 

HZ  F 

HZ  Q 

HXTJ 

LXTJ 

TO 

TF 

TMEASO 

D THE  AS 

DTPRNT 

DTCCPL 

DTPRPL 

DTSTIX 

DTNOYS 

L  PR 

LPRXS 

LPRXF 

LPRDG 

LPRLT 

LPRUD 

LPRZR 

L  PR  M 

L  PR  1C 

LPRXTJ 

LCC 

LPP 

LPPLD 

LPPUP 

ICO.NT 

ISEED 

I  PASS 

I  PR RUN 

IPGSIZ 

NODE 

TOLER 

HH  AX 

M  HIM 

HO 


Range 

>  0 
>  0 
>  0 
>  0 
>  0 
>  0 

T  o  r  F 
>  0 

>  TO 

>  0 
>  0 
>  0 
>  0 
>  0 
>  0 
>  0 

T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
T  or  F 
1  or  not  1 
Unlimited 
>  0 
>  0 
>  0 

T  or  not  1 

>  0 

>  HMIN 

>  0 
>  0 


De  faul  t 

1 

1 

0 

0 

0 

0 

F 

0.0 
1.0 
0.0 
1  .E+09 
1  .E+09 
1.E+0  9 
1  .  E  +09 
1  .E*09 
1  .E+09 
T 
T 
T 
T 
F 
F 
F 
F 
F 
F 
F 
F 
F 
F 
0 

77 

1 

1 

0 

1 

1  .E-4 
1.E+9 
1  .E-4 
1.E-2 
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4. 1.1. 2  Pr  inter  Plot  Specification 


S  OF  E  was  designed  to  Make  up  to  20  printer  plots.  Each 
plot  is  a  time  history  of  user- spec i fi ed  variables  as  they 
evolved  in  the  first  run.  The  data  for  Making  the  specified 
plots  are  saved  on  TAPE7  during  the  first  run  and  then  plot¬ 
ted  after  the  last  run.  Data  sampling,  which  occurs  at  TO, 
at  DT  PR  PL  intervals,  at  update  tiMes  (if  LPPUP  is  true)  and 
at  TF,  is  discontinued  after  101  samples  since  101  points 
saturate  the  time  axis  resolution.  Note  that  when  LPPUP  is 
true,  each  update  produces  three  samples  at  ti  (one  each  at 
ti",  t 1 +  and  ti+c)  which  can  rapidly  fill  the  101  available 
sample  slots.  Printer  plots  are  shown  in  Appendices  A  and 
B. 


Clearly  printer  plots  are  meant  to  provide  only  a 
quick-look  capability.  They  will  not  satisfy  the  need  for 
ensemble-of-runs  plots  or  statistical  calculations.  This 
need  is  met  by  SOFEPL  C5D  as  discussed  in  4.2.2.  If  a  user 
wants  to  go  to  the  trouble  of  writing  more  code,  ensemble 
statistics  can  also  be  computed  in  user-supplied  routines  as 
illustrated  in  subroutine  ESTIX  of  the  satellite  orbit  prob¬ 
lem,  Section  5.2  and  Appendix  B. 

Each  printer  plot  is  specified  by  a  4-tuple  called  the 
'plot  parameter  set'.  This  4-tuple  specifies  what  to  plot 
against  time  and  how  the  plot  data  are  to  be  scaled.  To  11- 
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lustrate,  suppose  (for  sone  perverse  reason)  one  wanted  to 
plot  the  difference  Xs(5)  -  Xf(4)  surrounded  by  ^S9RT(Pf(7)) 
with  a  scale  factor  of  100  on  all  curves;  the  plot  parame¬ 
ter  set  would  be  (5, 4, 7, 1 00. ) . 


In  general,  a  plot  parameter  set  is  in  the  form  NXS, 
NXF  ,  NPF  ,  R  where  NXS  is  the  index  for  the  required  X_s  vari¬ 
able,  etc.,  and  R  is  the  scale  factor  applied  equally  to 
every  curve.  Hissing  variables  are  Indicated  by  a  0  i n  a 
set.  For  example,  (5, 0,0, 0.3)  plots  0.3*Xs(5)  versus  time. 
Table  4-4  shows  which  curves  are  drawn  for  the  six  allowable 
combinations  of  NXS,  NXF,  NPF,  R. 


Table  4-4 


PRINTER  PLOT  PARAMETER  SET  DEFINITIONS 


Parameter  Set 

1  nxs,  3~7 

2  NXS,  0  , NPF ,R 

3  0  , NX  F ,  0  , R 

4  0  , NXF, NPF, R 

5  NXS, NXF, NPF, R 

6  NXS, NXF,  0  , R 


Curve(s)  Drawn  (R  Factor  Omitted) 

xITnxs ) 

Xs (NXS ) ,Xs (NXS )+ SORT (Pf (NPF) ) 

Xf (NXF) 

Xf (NXF) ,Xf (NXF)  +  SQRT<Pf (NPF)  ) 

Xs (NXS )-Xf (NXF) ,+SQRT(Pf  (NPF) ) 
Xs(NXS),Xf (NXF) 


4.1.2  TAPE  3  Input,  External  Tr aj  ectory 

T APE 3  contains  unformatted  records  that  are  used  to 
supply  external  trajectory  Information.  An  example  of  TAPE 3 
use  would  be  to  supply  exact  whole-valued  position,  velocity 
and  attitude  for  the  simulation  of  a  navigation  system. 
TAPES  contains  a  header  (optional)  followed  by  fixed-length 
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records  in  binary  format.  If  the  trajectory  is  either 
internal  or  not  required,  LXTJ  is  set  FALSE  and  TAPE3  is  not 
used  . 


SOFE  begins  each  run  by  rewinding  TAPE3  and  calling 
TRAJO  which  is  responsible  for  reading  the  TAPE3  header. 
Doing  things  this  way  allows  the  user  to  write  any  header  he 
desires  since  he  must  also  write  TRAJO.  The  header  should 
probably  be  echoed  to  T  APE  6  listable  output,  but  only  on  run 
one. 


After  TRAJO  has  read  the  user's  header,  SOFE  will  begin 
to  access  trajectory  data.  Each  fixed-length  trajectory 
record  is  now  read  using  a  FORTRAN  unformatted  read  state¬ 
ment  like  the  following: 

READ  (3)  T, (DUMMY (I ), 1=1 ,  NXT  J  ) 

Each  TAPE 3  record  must  contain  time  followed  by  NXT J  vari¬ 
ables.  Recall  that  NXTJ  is  specified  to  SOFE  in  PRDATA. 
The  NXTJ  variables  are  those  chosen  by  the  user  and  placed 
on  TAPE  3  for  his  particular  problem. 

The  TAPE3  read  process  occurs  in  subroutine  SPAN. 
61ven  simulation  time  T,  SPAN  surrounds  T  with  trajectory 
data  from  three  consecutive  TAPE3  times  (T1,T2,T3>  such  that 
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T1  <_  T  <  T  2  <  T3.  If  this  inequality  cannot  be  satisfied  or 
if  SPAN  runs  into  an  unexpected  end-of-file  (EOF),  SPAN 
halts  the  program  and  writes  a  diagnostic  message. 

After  SPAN  has  acquired  the  correct  data,  subroutine 
INTERP  interpolates  that  data  using  cubic  splines. 
Interpolation  occurs  at  points  in  [T1,T33  where  the  integra¬ 
tor  needs  derivative  evaluations  of  £s,  Xf  and  Pf.  Current 
interpolated  data  are  made  available  to  the  user  by  passing 
them  in  the  argument  list  of  every  user  routine  except 
USRIN.  See  Section  4.3  and  Table  4-5. 


The  following  are  points  to  remember  about  the  external 
trajectory  capability: 


o  TAPE3  records  need  not  be  equally  spaced  in  time 
but  they  must  be  in  (ascending)  chronological  order. 


o  The  time  spacing  of  TAPE3  data  should  be  close 
enough  to  portray  the  activity  in  the  trajectory. 
Shannon's  sampling  theorem  applies  here.  However, 
oversampling  could  be  costly  in  computer  time  be¬ 
cause  SOFE  uses  every  TAPE3  record  it  sees. 


o  TO  must  not  be  less  than  the  first  TAPE3  time. 


o  The  next  to  last  T APE 3  time  must  be  strictly 
greater  than  TF. 


These  last  two  conditions  may  be  summarized  as  follows: 
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mlnTI  <  TO  <  TF  <  maxT2 


(4-1  ) 


4.1.3  T  APE  9  Input  ,  Pr  tvlam  Pr  ob  L  eg  Cont  1 nul ng 

When  the  present  problem  Is  the  continuation  of  a  pre¬ 
vious  problem,  TAPE9  Is  required  to  supply  Xso,  Xfo  and  Pfo 
data  to  restart  each  run,  IRUM*1 ,2, . . .,  IPASS.  If  this  Is  a 
brand  new  problem,  not  related  to  any  previous  problem, 
ICONT  should  be  set  to  a  value  other  than  1  to  show  TAPE?  is 
not  required. 

When  TAPE  9  is  required.  It  must  contain  the  following 
sequence  of  unformatted  variable- length  records: 


Use 

Used  to  continue  run  1 


Used  to  continue  run  2 


Used  to  continue  run  IPASS 


TAPE9  will  be  configured  this  way  automatically  If  It  is  the 
TAPE10  snapshot  output  from  a  previous  run.  SOFE  will  read 
TAPE?  in  subroutines  GETX  and  6ETPF  using  the  READ  (9)  ... 
statement. 
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4 . 2  Out  put 


This  section  has  four  parts,  one  for  each  of  the  four 
tapes  (numbered  4,  6,  8  and  10)  that  SOFE  can  output.  The 
records  on  these  tapes  contain  periodic  summaries  of  the 
state  of  the  truth  and  filter  models.  The  output  rate  to 
each  tape  (except  TAPE10)  can  be  set  independently  by  using 
control  parameters  in  the  PR  BATA  group. 

4.2.1  T  APE  6  Output ,  Li  stable  Information 

Llstable  output  records  consist  of  the  following  in  the 
given  order.  Appendices  A,  B  and  C  provide  examples  of 
llstable  output. 


a .  Banner,  UPDATE  Summary,  Da  te  and  Time  -  The  C D C 
banner  page  appears  first  followed  by  a  page  summar¬ 
izing  the  subroutines  and  corrections  in  the  SOFE 
deck.  This  page  is  generated  by  the  UPDATE  utility. 
At  the  bottom  of  this  page  are  the  date  and  time  of 
the  r un . 

b .  SOFE  Header  Page  -  Contains  general  words  naming 
the  program,  echoing  the  user's  title,  and  again 
giving  the  run  date  and  time. 

c.  Hamel  1 st  and  Nonze  ro  Elements  -  Contains  the 
PR  DA  T A  list  of  40  parameters  and  two  statements  to 
echo  the  row-column  pairs  of  the  nonzero  elements  of 
F  and  Qf 

d.  Un l abe l ed  Common  -  The  next  page  of  output  be¬ 
gins  with  a  table  showing  the  structure  of  blank 
common  area  'A*.  A  statement  comparing  the  present 
size  of  A  to  Its  required  size  appears  after  the 
table.  If  A  is  too  small,  SOFE  prints  a  message  to 
this  effect,  tells  the  user  to  Increase  the  size  of 
A  by  altering  two  statements  in  the  main  program, 

and  STOP'S.  A's  present  size  Is  1000. 

/ 

e.  User  Input  -  Anything  written  by  USRIN  or  by  the 
entry  pKase  of  the  other  eight  user-written  routines 
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is  out  put  next. 


f.  Run  Initialization  Header  -  Each  listed  run  be¬ 
gins  with  the  user's  title  followed  by  run  date,  run 
time  and  run  number. 

g.  Initial  Va lue  s  Echoed  -  Xso,  Xfo,  SQR  T  [Pf  o ( i  ,  i  )  3 
and  Pfo  are  displayed  at  TO  before  problem  execution 
begins.  FALSE  settings  for  LPR  or  for  LPRXS,  LPRXF, 
LPRDG  and  LPR  LT  will  suppress  all  or  any  of  these 
output  s . 

h.  Periodic  Solution  Printout  -  At  DT  PR  NT  intervals 
and  at  filter  update  times  solution  records  are 
listed.  The  contents  of  these  records  are  governed 
by  the  ten  print  control  parameters  that  begin  LPR. 
At  DTPRNT  intervals  the  maximum  listed  output  is  Xs, 
Xf,  the  sigma  value  for  each  filter  state,  Pf  shown 
Tn  a  lower  triangular  display  and  the  interpolated 
trajectory  data.  At  update  times,  if  LPR  UD  is  true, 
these  same  quantities  are  displayed  just  before  the 
update  (T - ) ,  Xf+  and  Pf+  are  displayed  after  the  up¬ 
date  (T  +  ) ,  ancf  £s  +  C  and  Xf  +  C  are  displayed  just 
after  the  feedback  correction  (T+C).  In  addition, 
the  measurement  residuals,  the  standard  deviations 
of  those  residuals,  the  H  matrix  computed  by  HR Z , 
and  the  Kalman  gain  may  be  displayed  at  update  time. 
All  output  is  in  units  specified  by  the  user  for  his 
problem,  e.g.,  in  meters,  seconds  and  radians. 
Units  conversion  is  not  done  by  SOFE. 

i.  Final  Values  -  Same  output  set  as  at  TO  is 
printed  at  TF. 

j.  Printer  Plots  -  Up  to  20  plots  are  generated  as 
specified  by  the  plot  parameter  sets.  If  LPPLD  is 
TRUE,  the  point  pairs  for  each  plot  are  also  listed. 

k .  Final  Check  Pr oduc  t  -  This  scaler  is  formed  by 
chaining  together  In  a  single  product  all  the  non¬ 
zero  elements  of  X3,  £f  and  Pf  at  TF.  This  product 
is  a  handy  index  for”  checking  whether  a  particular 
simulation  run  changed  unexpectedly. 


The  repeatable  output  consists  of  items  f.  through  i. 
which  recur  on  every  simulation  run  until  the  run  number 
exceeds  IPRRUN.  The  only  output  for  runs  after  IPRRUN  is  a 
simple  statement  that  the  run  was  completed.  All  f. 
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through  i.  output  is  commanded  to  TAPE6  from  the  output  ex¬ 
ecutive  routine  'OUT'. 

Subroutine  PAGCON  maintains  page  control  of  listable 
output.  All  periodic  print  commands  are  preceded  by  a  call 
to  PAGCON  indicating  1)  the  number  of  lines  to  print  and  2) 
whether  an  a  priori  page  eject  should  occur.  When  re¬ 
quired,  PAGCON  issues  a  page  eject  of  its  own,  thus  main¬ 
taining  line  count  and  avoiding  the  breakup  of  output  over  a 
page  boundary.  When  a  printer  has  page  size  controls  built 
in,  one  can  save  some  computation  time  by  setting  IPGSIZ  to 
zero,  thereby  disabling  all  calls  to  PAGCON. 

4. 2. 1.1  Diagnostic  Out  put 

In  addition  to  the  planned-for  output  cited  above,  a 
diagnostic  message  is  listed  when  SOFE  detects  a  processing 
error.  Tabulated  below  are  the  subroutines  that  produce 
such  messages,  together  with  the  errors  they  detect.  All 
these  errors  are  fatal  to  further  execution,  so  SOFE  is 
STOPed  if  one  occurs. 


ADVANS: 

Integration  failed  in  KUTMER 

GETPF: 

IROW  or  ICOL  >  NF  on 

input 

INTERP: 

Spline  construction 

failed 

INTERP: 

Spline  evaluation  failed 

PL  PR : 

Wore  than  20  printer 

plots  requested 
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PL  PR  : 

PS  ®R  T : 

SOFEIN: 

SPAN: 

SPLITA: 

VALOTA: 


Insufficient  data  cards 

Covariance  not  positive  semidefinite 

A  dimension  or  size  is  out-of-range 

TAPE3  data  not  in  sync 

Blank  COMMON  too  small 

An  input  parameter  is  out-of-range 


The  printed  error  message  usually  contains  some  information 
to  help  pinpoint  the  problem.  In  addition,  any  routine  that 
reads  input  checks  each  FORTRAN  READ  for  an  EOF  and  STOP'S 
the  program  when  any  EOF  is  found.  The  input  subroutines 
are  GETPF,  GETX,  NZRCIO,  SOFEIN  and  SPAN. 

About  half  of  the  above-listed  processing  errors  arise 
from  very  specific  flaws  that  are  easily  fixed,  e.g. 
increasing  the  size  of  blank  COMMON  array  A  (and  the  DATA 
statement  for  MAXA)  fixes  the  problem  detected  in  SPLITA. 
Note  that  the  dimension  of  array  A  must  be  no  smaller  than 
this: 


4NF**2  ♦  15NF  ♦  7NS  ♦  2H  ♦  3(NZF+NZQ>  ♦  13NXTJ  ♦  3 

The  remainder  of  the  error  messages  arise  for  a  variety  of 
reasons  that  are  not  easily  characterized.  A  case  in  point 
is  an  Integration  failure.  This  error  may  occur  due  to  an 
inappropr iate  choice  of  the  pair  TOLER  and  HMIN,  or  to  an 
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incorrect  specification  of  derivatives  in  XFDOT  or  XSDOT, 
such  as  the  insertion  of  a  step  change  in  sose  rate.  This 
writer  has  not  seen  the  errors  cited  in  INTERP  and  SPAN 
occur  in  practice;  the  tests  remain  in  place  for  safety. 
An  indefinite  covariance  occurred  once  and  was  detected  by 
PSQRT;  it  was  caused  by  some  incorrect  off-diagonal  terms 
in  PF  input. 

4.2.2  TAPE  4  Output  ,  Ca  l  comp 

When  LCC  is  true,  TAPE 4  is  generated  to  be  the  input  to 
SOFEPL  C53,  the  postprocessor  program  for  the  plotting  of 
averages  formed  across  an  ensemble  of  Monte  Carlo  runs. 
TAPE  4  consists  of  variable  length  records  containing  time, 
Xs,  Xf,  the  diagonal  elements  of  Pf,  measurement  residuals 
and  residual  variances.  These  records  are  written  to  TAPE4 
using  unformatted  binary  writes.  Data  sampling,  which  oc¬ 
curs  at  TO,  DTPLCC  intervals,  update  times  (ti“,  ti+  and 
ti+c)  and  TF,  continues  for  all  IPASS  runs. 

Using  the  data  on  TAPE 4,  SOFEPL  can  make  16  different 
types  of  plots  (all  versus  time)  with  options  available  for 
scaling,  time  windowing  and  printing.  To  illustrate,  one 
can  plot  Ss(3),  Xf(6>,  E  *  Xs(4)-Xf(3),  SE  (which  is  the 
standard  deviation  of  E),  Pf  (3, 3)**(S.  5,  TrTSTTT,  etc.,  where 
the  overbar  indicates  an  ensembte  average  has  been  formed 
across  a  collection  ot  Monte  Carlo  runs  specified  by  the 
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user.  S  OF  E  PL  uses  the  computer  graphics  Language  DISSPLA  to 
generate  an  intermediate  file  that  may  Later  be  Linked  to  a 
Calcomp  plotter,  a  CRT  plotting  terminal,  or  other  graphics 
devices.  Some  plots  made  with  SOFEPL  are  shown  in  Appendix 
B. 


4.2.3  T  APE  8  Output ,  Use  r-De  f i ned 

TAPE8  is  provided  for  user-defined  output.  Examples  of 
such  output  might  be  error  differences,  normalized  error 
quantities,  data  to  interface  SOFE  with  another  processor, 
etc.  A  convenient  place  from  which  to  write  such  output  is 
user-written  subroutine  ESTIX  which  is  called  at  DTSTIX  in¬ 
tervals.  Basic  SOFE  does  nothing  with  T  A  PE  8 . 

4.2.4  TAPE  1 0  Output,  Final  Snapshot 

TAPE10  is  generated  automatically  by  SOFE.  At  problem 
completion  it  contains  a  complete  set  of  Xs(TF),  Xf(TF)  and 
Pf(TF)  values  for  each  run.  TAPE  1 0  is  closed  with  an  EOF 


mark  after  the  IPA$St_£  run  is  complete.  The  use  of  this 
data  for  problem  continuation  is  discussed  in  4.1.3. 


4.3  User-Written  Subroutines 


Basic  S OF E  consists  of  31  routines  that  are  constant 
from  one  problem  to  the  next.  SOFE  'adapts1  to  new  problems 
by  accepting  user-written  subroutines  that  define  the  vari¬ 
able  aspects  of  every  new  application.  This  section  out¬ 
lines  the  purpose  and  minimum  requirements  for  each 
user-written  subroutine.  We  list  their  names  below: 


AMEND 

HRZ 

USRIN 


ESTIX 

SNOYS 

XFDOT 


FQGEN 
TRAJO 
XS  DOT 


Each  user  routine  is  a  FORTRAN  subroutine.  Except  for  USRIN 
and  TRAJO,  each  must  contain  a  FORTRAN  ENTRY  statement.  The 
entry  name  is  formed  by  adding  an  'O'  to  the  subroutine's 
name,  e.g.,  AMENDO  from  AMEND.  These  entry  names  are  called 
once  at  the  beginning  of  each  run  iri  order  to  initialize  the 
data  or  variables  particular  to  that  routine.  The  run 
number  (1 , 2, . . ., I  PA SS )  is  supplied  in  the  argument  list  to 
allow  the  user  to  modify  or  inhibit  this  initialization  as 
he  might  desire  from  run  to  run. 


Definitions  for  the  parameters  and  variables  that  ap¬ 
pear  In  the  argument  lists  of  the  user  routines  are  given  in 
Table  4-5.  The  first  twelve  quantities  In  Table  4-5  are  for 
input  and  must  not  be  altered  in  any  user  routines.  The 
routines  that  may  output  and  thereby  alter  the  last  eight 
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quantities  in  Table  4-5  are  mentioned  in  the  individual  des¬ 
criptions  that  follow  next.  Examples  of  each  routine  are 
given  in  Appendices  A  and  B. 

PF,XF  and  XS  are  FORTRAN  names  for  Pf,  X^f  and  X.s*  H, 
rF  and  ZRES  are  FORTRAN  names  for  Hj,  Rfj  and  DZj  as  defined 
by  equations  (2-33),  <2-34)  and  (2-35)  respectively.  F  and 
QF  are  FORTRAN  names  for  F(t;X^f)  and  Qf(t)  as  defined  by 
(2-16)  and  the  remarks  after  (2-4)  respectively.  XDOT  is 
the  homogeneous  part  of  >LS  or  &f;  it  is  computed  as  jj(Xs,t) 
in  subroutine  XSDOT  and  as  _f(Xf,t)  in  subroutine  XFDOT. 
Naturally,  the  user  is  free  to  alter  any  of  these  FORTRAN 
names  to  better  suit  his  problem  or  his  preferences.  The 
FORTRAN  names  in  Table  4-5  will  be  used  in  the  user-routine 
descriptions  that  follow  now  and  also  in  coding  for  the  two 
examples  in  Section  5. 

4.3.T  ABEND 

Subroutine  AMEND  is  called  at  ti  (after  all  M  measure¬ 
ments  have  been  processed)  to  apply  feedback  of  newly  com¬ 
puted  estimates  to  both  filter  and  truth  states.  A  typical 
use  of  this  routine  would  be  to  implement  total,  impulsive 
control  in  which: 

o  All  filter  estimates  are  zeroed:  XF,new  *  0. 

o  The  corresponding  truth  states  are  bumped  by  the 
same  amount  as  the  filter  states  changed. 
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If  the  user  wished  to  Implement  an  open  loop  system,  he 
would  do  nothing  to  alter  either  XS  or  XF  in  AMEND.  Other 
feedback  schemes,  including  continuous  control,  can  be 
achieved  by  using  AMEND  in  conjunction  with  the  derivative 
routines  XSDOT  and  XFDOT.  Table  4-6  shows  the  layout  of 
AMEND  with  the  required  statements  in  capital  letters. 

Table  4-6 

REQUIRED  STATEMENTS  FOR  AMEND 

SUBROUTINE  AME ND (IRUN, T, NF , NS, NXT J , X F , XS, XTRAJ ) 
DIMENSION  XF (NF ), XS (NS), XTRAJ  (NXT J) 
feedback  computations 
RETURN 

ENTRY  AMENDO 

initialization 

RETURN 

END 

4.3.2  ESTIX 

Subroutine  ESTIX  is  called  at  DTSTIX  intervals  to  com¬ 
pute  whatever  quantities  the  user  desires.  TAPE 8  is  provid¬ 
ed  for  storing  these  quantities  If  so  required.  Table  4-7 
shows  the  layout  of  ESTIX  with  the  required  statements  in 
capital  letters. 


Table  4-7 

REQUIRED  STATEMENTS  FOR  ESTIX 

SUBROUTINE  EST IX (IRUN,T, NF ,NS, NXT J , XF, XS, XTRAJ, 
*  NTR, PF ) 

DIMENSION  XF <NF ) , XS (NS), XTRAJ (NXT J ) ,PF (NTR > 

computations  for  user  quantities 

RETURN 

ENTRY  ESTIXO 
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initialization 

RETURN 

END 

4.3.3  FOSE  N 

Subroutine  FQGEN  computes  the  values  for  the  nonzero 
elements  of  the  filter  matrices  F  and  QF  where  F  is  .efined 
by  equation  (2-16)  and  QF  by  equation  (2-4).  F  and  QF  are 
used  in  FPPPFT  and  ASYSP  to  compute  the  derivative 

PF  =  F*PF  +  PF*FT+  QF  (2-28) 

It  is  important  to  note  that  the  indices  assigned  to  the 
nonzero  values  of  F  and  QF  in  FQGEN  must  agree  with  the 
order  in  which  the  nonzero  row-column  pairs  were  specified 
on  input.  That  is,  using  F  for  illustration,  the  first  non¬ 
zero  row-column  pair  is  associated  with  F(1),  the  second 
with  F(2),  etc.  Any  order  is  allowed  so  long  as  the  nonzero 
row-column  pair  order  and  the  F  element  index  order  agree. 

Table  4-8  shows  the  layout  of  FQGEN  with  the  required 
statements  in  capital  letters.  Note  that  the  ENTRY  area  is 
a  convenient  and  efficient  place  for  assigning  the  time  in¬ 
variant  values  of  F  and  QF.  Also  note  that  FQGEN  is  in  the 
Innermost  integration  loop  which  means  it  is  called,  along 
with  XFDOT  and  XSDOT,  much  more  frequently  than  most  other 
routines.  The  user  should  therefore  endeavor  to  write  effi- 
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cient  code  In  constructing  FQGEN,  XFDOT  and  XSDOT. 

Table  4-8 

REQUIRED  STATEMENTS  FOR  FQGEN 

SUBROUTINE  F QGE N <1 R UN, T, NF , NS, NXT J , XF , XS, XTRAJ, 

*  NZ F,NZQ, F,QF) 

DIMENSION  XF (NF) ,  XS (NS), XT  RAJ (NXT  J )  , F(NZ  F) ,QF (NZQ) 
computations  tor  nonzeroes  of  F  and  QF  in  proper 
order 
RETURN 

ENTRY  F QGE  NO 

initialization 

RETURN 

END 

4.3.4  HRZ 

Subroutine  HRZ  is  called  M  times  at  each  measurement 
update  time  to  compute  values  for  the  vector  H  and  the  sca¬ 
lars  RF  and  ZRES.  The  user  schedules  measurement  updates  at 
desired  times  by  proper  choice  of  the  input  parameters 
TMEASO  and  DTMEAS.  He  car.  suppress  a  particular  measurement 
at  any  update  time  by  returning  a  negative  RF  which  SOFE  in¬ 
terprets  as  a  command  to  increment  IMEAS  and  then  proceed 
immediately  to  the  next  measurement.  In  short,  every  update 
session  results  in  M  calls  from  SOFE  to  HRZ,  with  the  update 
algebra  invoked  in  SOFE  after  any  call  in  which  RF  is 
nonnegative.  A  varied  measurement  schedule  can  be  arranged 
by  the  appropriate  combination  of  input  parameter  choices 
and  logic  in  HRZ. 
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Function  subprogram  GA US S ( AME AN, S TD )  Is  available  for 
computing  random  samples  from  a  Gaussian  distribution  to  aid 
in  constructing  ZRES.  AMEAN  and  STD  are  the  mean  and  stan¬ 
dard  deviation  of  the  desired  distribution.  Table  4-9  shows 
the  layout  of  HRZ  with  the  required  statements  in  capital 
letters. 

Note  that  PF,  one  of  the  formal  parameters  in  the  HRZ 
argument  list,  is  not  used  in  either  the  linear  or  the  ex¬ 
tended  Kalman  filter.  It  is  there  because  most  higher-order 
filter  mechanizations,  e.g.  the  Gaussian  second-order 
filter,  require  it  to  form  a  bias  term  for  compensating 
ZRES.  PF  is  also  included  in  the  argument  list  of  XFDOT  in 
anticipation  of  higher-order  filter  needs. 

Table  4-9 

REQUIRED  STATEMENTS  FOR  HRZ 

SUBROUTINE  HR Z ( IR UN, T, NF , NS, NXT J , XF , XS, XTRAJ,NTR, PF, 
*  IMEAS,H,H,RF,ZRES) 

DIMENSION  XF(NF)  ,XS  (NS),  XT  RAJ  (NXT  J  )  ,  PF  (N.TR  )  ,H  (N  F  ) 

computations  for  H,RF  and  ZRES 

RETURN 

ENTRY  HRZO 

initialization 

RETURN 

END 

4.3.5  SNOYS 

Subroutine  SNOYS  is  called  at  DTNOYS  Intervals  to  allow 
the  user  to  Inject  noise  into  the  appropriate  states  of  XS. 
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These  are  the  truth  states  influenced  by  the  ws(t)  ter*  in 
equation  (2-1).  The  usual  procedure  here  is:  first,  obtain 
a  delta  covariance  based  on  Qs  that  accounts  for  the  random 
portion  of  the  growth  of  the  covariance  of  the  XS  process 
over  the  previous  DTNOYS  seconds;  second,  using  this  delta 
covariance,  sample  from  a  Gaussian  distribution  to  obtain  a 
random  sample  wd;  and  third,  add  wd  to  XS  to  produce  a  per¬ 
turbed  truth  state.  The  sampling  procedure  can  use  function 
GAUSS,  which  was  described  in  HRZ  previously.  Table  4-10 
shows  the  layout  of  SNOYS  with  the  required  statements  in 
capital  letters. 


Table  4-10 

REQUIRED  STATEMENTS  FOR  SNOYS 

SUBROUTINE  S NO YS  < I R UN, T, NF , NS, NXT J , XF, XS , XTR A J ) 
DIMENSION  XF(NF) ,XS (NS), XT RAJ  (NXTJ  ) 
computations  to  Inject  noise  in  XS 
RETURN 

ENTRY  SNOYSO 

initialization 

RETURN 

END 


4.3.6  TRAJO 

The  phrase  'trajectory  data'  comes  out  of  the  INS  con¬ 
text  where  it  usually  means  true,  whole-valued  position, 
velocity  and  attitude.  Such  data  can  be  produced  during 
SOFE  simulation  runs  (e.g.  by  inclusion  In  XS )  or  drawn  In 
from  an  external  trajectory  tape.  The  latter  approach  is 
generally  preferred  because  it  reduces  the  computational 


86 


load;  SOFE  is  fully  ready  to  accommodate  this  approach  (see 
Subsection  4.1.2  on  T APE 3  input). 

However,  when  trajectories  need  to  be  generated  during 
SOFE  runs,  TRAJ  could  be  constructed  and  called  by  the  user 
to  do  the  job.  SOFE  itself  never  calls  TRAJ.  Therefore, 
unless  and  until  the  user  assigns  TRAJ  a  job  to  do,  it  is 
not  needed. 

TRAJO,  on  the  other  hand,  is  called  by  SOFE  at  the  be¬ 
ginning  of  each  new  run  to  initialize  TRAJ  if  necessary  or 
to  read  the  TAPE  3  header  if  LXTJ  is  TRUE.  Table  4-11  shows 
a  layout  for  both  TRAJ  and  TRAJO  (although  only  TRAJO  is  es¬ 
sential)  with  the  required  statements  in  capital  letters. 

Table  4-11 

REQUIRED  STATEMENTS  FOR  TRAJO 

SUBROUTINE  TRAJ ( I R UN, T, NF , NS, NXT J , X F , XS , XT R A J ) 

DIMENSION  XF(NF) ,XS  (NS),XTRAJ  (NXTJ) 

COMMON/. ../. ..(optional) 

computation  of  actual  trajectory  quantities  (optional) 

RETURN 
ENTRY  TRAJO 

initialize  actual  trajectory  (optional) 
read  TAPE3  header  (required  if  LXTJ  =  .T.) 

RETURN 

END 

4.3.7  USRIN 

Subroutine  USRIN  is  called  once  by  SOFE  during  problem 
Initialization  for  the  primary  purpose  of  reading 
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user-supplied  data.  However,  USRIN  could  be  used  to  perform 
any  function  that  needed  to  be  done  only  once  at  problem 
startup.  If  card  input  data  are  to  be  read  by  USRIN,  those 
data  must  be  inserted  between  the  initial  covariance  data 
and  the  printer  plot  data  (see  Table  4-2).  Table  4-12  shows 
the  layout  of  USRIN  with  the  required  statements  in  capital 
letters.  Note  that  USRIN  has  no  ENTRY  statement. 

Table  4-12 

REQUIRED  STATEMENTS  FOR  USRIN 

SUBROUTINE  USRIN 
read  user  data 
RETURN 
END 

4.3.8  XFDOT 

Subroutine  XFDOT  computes  the  homogeneous  part  of  the 
derivative  XDOT  of  the  filter  state  vector  XF.  This  deriva¬ 
tive  is  given  by  (2-27)  as 

XDOT  =  £(XF,t)  (2-27) 

XDOT  output  is  used  to  propagate  the  filter  state  vector 
between  updates  via  numerical  integration.  As  mentioned  be¬ 
fore  in  4.3.3,  this  routine  is  called  often,  so  efficiency 
of  coding  here  could  significantly  shorten  run  time. 
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We  remark  that  (2-27)  Is  the  most  general  equation, 
descriptive  of  the  nonlinear  situation  when  the  extended 
Kalman  filter  is  appropriate.  If  (2-27)  can  be  reorganized 
a  s 


XDOT  =  F(t)*XF 


(4-2) 


linear  filter  principles  apply  and  the  user's  job  is  simpli¬ 
fied.  To  wit: 


o  The  nonzero  values  of  F(t)  computed  for  (4-2)  are 
the  ones  required  for  F  in  FQGEN.  Sharing  of  these 
values  through  a  user-defined  labeled  COMMON  area 
should  result  in  computer  savings.  Note  that  XFOOT 
is  called  before  FQGEN. 


Note  that  basic  SOFE  makes  no  distinction  between  linear  and 
nonlinear  problems.  All  such  differences  are  manifested  in 
the  user's  computations  for  F,  Ih,  H  and  ZRES.  Table 
4-13  shows  the  layout  of  XFDOT  with  the  required  statements 
i n  capi tal  letters. 


Table  4-13 

REQUIRED  STATEMENTS  FOR  XFDOT 

SUBROUTINE  XFDOT(IRUN,T,NF,NS,NXTJ,XF,XS,XTRAJ, 

*  NTR,PF,XDOT> 

DIMENSION  XF (NF) ,XS (NS) ,XTRAJ <NXTJ),PF (NTR) ,XDOT (NF> 

computations  to  form  XDOT 

RETURN 

ENTRY  XFOOTO 

initialization 

RETURN 

END 
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4.3.9  XSOOT 


Subroutine  XSOOT  computes  the  homogeneous  part  of  the 
derivative  of  the  truth  state  vector  XS.  This  derivative, 
which  again  carries  the  FORTRAN  name  XDOT,  is  given  from 
(2-1)  as 


XDOT  =  5  (XS,t>  (4-3) 

XDOT  is  used  to  propagate  the  truth  state  via  numerical  in¬ 
tegration  between  noise  addition  points.  As  with  FQGEN  and 
XFDOT,  XS  DOT  is  called  often,  so  efficiency  in  coding  is  im¬ 
portant.  Table  4-14  shows  the  layout  of  XSDOT  with  the  re¬ 
quired  statements  in  capital  letters. 

Table  4-14 

REQUIRED  STATEMENTS  FOR  XSDOT 

SUBROUTINE  XSDOT (IRUN,T,NF,NS,NXTJ , X  F  ,XS ,  XT  R  AJ  ,XD OT  ) 
DIMENSION  XF (NF) ,XS (NS) , XT  RAJ  (NXTJ ) ,XDOT  (NS) 
computations  to  form  XDOT 
RETURN 

ENTRY  XSDOTO 

initialization 

RETURN 

END 

4.3.10  Summary  of  User-Wr i tten  Routines 

The  following  is  a  short  summary  of  Subsection  4.3.  It 
provides  a  brief  definition  of  what  calculations  each 
user-written  routine  must  perform  and  gives  the  appropriate 
equations  as  required. 
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oAWEND 


:Apply  Feedback 


oESTIX 

:User- Defined  Output 


0FQ6EN 

:Calculate  Nonzero  Values  of  F  and  QF 


f  (t) 


dl (Xf,t> 
dXf 


Xf 


ECwf  <t)wf^t  +  T)3  =  Qf(t)  *  5(T) 


oHRZ 


:  Calculate  H,  Assign  RF,  Simulate  ZRES 
dhf(Xf,ti) 


H  ( t  i  )  * 


<*Xf 


Xf  =  Xf 


ECvf  (ti)vf  (tj)3  =  Rf<t)*$ij 

ZRES  =  [  hs(Xs,ti)  +  vs  -  hf(Xf,ti) 
H s *X s  +  vs  -  H*Xf 


oSNOYS 


:  Inject  Additive  Noise  _wd  Into  Truth  States 
ECwd(tj)wdr(tj)3  *  Qs(tj)  *  At 


nonlinear 

linear 
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oTRAJ 


:  Supply  Trajectory  Data  for  Computing 
F,9f ,£s,£f *h.s,^f ,  etc. 

oUSRIN 

:  User-Oefined  Input 

oX  FOOT 

:  Calculate  Filter  Derivatives  (Homogeneous  Part  Only) 
if  8  I  _f(Xf,t)  nonlinear 

]  F(t)*Xf  linear 


oXSPOT 

:  Calculate  Truth  Derivatives  (Homogeneous  Part  Only) 
Xs  =  £(Xs,t) 


XS 


£(Xs,t ) 
G ( t ) * Xs 


nonlinear 
l  j  near 


9 
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5.0  EXAMPLE  PROBLEMS 


This  section  presents  examples  of  SOFE  and  SOFEPL  use 
for  two  simple  problems.  The  first  example,  taken  from  [ID 
and  C2D,  is  for  a  totally  linear  system.  The  second  example 
is  a  nonlinear  orbit  determination  problem  that  is  studied 
using  extended  Kalman  filter  techniques.  For  each  example 
we  describe  the  truth  model  and  the  reduced-order  filter 
model,  summarize  the  example’s  implementation  in  SOFE,  and 
discuss  the  results. 

5.1  Linear  System  Example 

This  linear  example  is  a  simplified,  undamped, 
single-axis  inertial  navigation  system  (INS)  having  position 
and  velocity  measurements  for  outputs.  In  general,  the 
navigation  equations  for  an  INS  are  nonlinear.  By  confining 
our  attention  to  the  error  states  of  this  simplified  INS,  we 
construct  a  purely  linear  example. 

5.1.1  Truth  Mode l 

The  model  for  the  truth  system  is  shown  in  Figure  5-1. 
The  truth  system  consists  of  a  single-axis  INS  driven  by 
gyro  and  accelerometer  sensor  errors.  In  the  figure,  R  is 
earth's  radius  and  g  is  acceleration  due  to  gravity.  The 
system  outputs  are  biased  measurements  of  position  and  velo¬ 
city  that  occur  every  30  seconds. 
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Figure  5-1.  INS  Truth  Model  Block  Diegrem 


The  basic  INS  truth  system  consists  of  position  error 
(si)/  velocity  error  (s2)/  and  tilt  error  (s3).  Tilt  is 
driven  by  two  random  drift  processes;  the  first  is  a 
first-order  Markov  process  (s4>  while  the  second  is  a  random 
constant  (s6>.  Velocity  error  is  driven  by  two  random  ac¬ 
celeration  processes,  both  first-order  Markov,  one  having  a 
long  correlation  time  (s7)  and  one  a  short  correlation  time 
Cs5).  The  other  two  states  in  the  truth  model  are  biases  on 
the  measurements.  Both  measurement  biases  are  first-order 
Markov  processes  with  s8  being  the  bias  on  position  and  s9 
the  bias  on  velocity.  The  above  information  is  summarized 
in  Table  5-1  where  data  on  the  nature  and  statistics  of  each 
random  process  are  also  given. 


Table  5-1 


DEFINITION 

i  OF 

TRUTH  MODEL 

STATES 

State 

Desc  r i pt 1  on 

Process 

Sigma  Correlation 

lis± 

Value 

Time 

9  1 

Position  error 

- 

- 

- 

s2 

Velocity  error 

- 

- 

- 

S3 

Tilt  error 

- 

- 

- 

s4 

Drift  bias 

1st 

Markov 

.01  deg/hr 

3600 

s5 

Accel .  bias 

1st 

Markov 

200E-6  g's 

300 

s6 

Drift  bias 

Random  constant 

.005  deg/hr 

infinite 

s7 

Accel .  bias 

1st 

Markov 

100E-6  g's 

3600 

s8 

Pos.meas.bi as 

1st 

Markov 

300  ft. 

1800 

s9 

Vel  .meas.bi as 

1st 

Markov 

0.5  ft/sec 

300 
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Denoting  )<s  as  the  set  of  states  (si ,s2,  . .  .,s9)  ,  the 
differential  equation  for  the  truth  system  is  linear  and  may 
be  written  in  state- space  form  as: 


0 

1 

0 

0 

0 

0 

0 

0 

0 

XS 

0 

0 

0 

-g 

0 

1 

0 

1 

0 

0 

0 

0 

1/R 

0 

1 

0 

1 

0 

0 

0 

•f 

0 

0 

0 

0 

-1  /3600 

0 

0 

0 

0 

0 

ws  1 

0 

0 

0 

0  - 

1  /300 

0 

0 

0 

0 

ws2 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-1  /  3600 

0 

0 

ws3 

0 

0 

0 

0 

0 

0 

0  - 

1  / 1 800 

0 

w  s  4 

0 

0 

0 

0 

0 

0 

0 

o  - 

1  /300 

ws5 

(5-1  ) 


The  vector  output  equation  is  also  linear  in  )(s 


Zs  =  [Zposl 
[Zve  IJ 

=  fl  0  0  0  0  0  0  1  ol  Xs  ♦  vs  (5-2) 

Lp  1  000000  1J 


The  two  measurements  are  uncorrelated  and  have  covariance 


Rs 


[ 


1 00  .  **2 
0 


0  1 
0 . 5  *  *  2  J 


(5-3) 


where  position  and  velocity  units  are  feet  and  ft/sec.  Note 
that  (  )**2  means  (  )  squared. 

5.1 .2  Fitter  Model 

One  primary  concept  for  a  filter  model  is  that  it 
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should  be  computationally  simple.  This  is  usually  accom¬ 
plished  in  part  by  deleting  states  from  the  truth  model  that 
are  deemed  non-significant.  In  the  case  at  hand,  the  ast 
four  states  of  £s,  viz.  s6  through  s9,  were  dropped,  leav¬ 
ing  a  dynamic  model  for  £f  like  that  for  truth  states  si 
through  s5.  Compensation  for  this  mismodeling  would  typi¬ 
cally  include  increasing  the  noise  at  the  ports  where  the 
deleted  states  drove  the  system  and  increasing  the  measure¬ 
ment  error  variance.  Neither  compensation  will  be  used  in 
this  implementation  so  we  may  obtain  results  comparable  to 
those  of  Cl]  and  C2J. 


The  filter  model  equations  corresponding  to  (5-1)  and 
(5-2)  are  therefore 


0 

1 

0 

0 

0 

Xf  + 

0 

0 

0 

-g 

0 

1 

0 

0 

1/R 

0 

1 

0 

0 

0 

0 

0 

-1 /3600 

0 

wfl 

0 

0 

0 

0  - 

1  / 300_ 

wf  2 

r 1 

0  0 

0 

0~]  Xf  + 

vf 

Lo 

1  0 

0 

Oj 

(5-4) 


(5-5) 


The  strengths  of  the  noises  wfl  and  wf2  were  chosen  to  equal 
those  of  wsl  and  ws2.  Similarly,  the  standard  deviations  of 
the  position  and  velocity  measurement  noises  were  left  un¬ 
changed  so  Rf  is 
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R  f 


0  1 

0. 5**2  I 


(5-6) 


[1  00 . *  *  2 
0 


The  initial  covariance 
listed  in  Table  5-2 . 
than  in  C1D  and  C  2  3  in 
the  covariance  output 
what  happens. 


matrix  is  diagonal  and  has  the  values 
Smaller  initial  values  were  used  here 
order  to  narrow  the  dynamic  range  of 
so  the  printer  plots  will  better  show 


Table  5-2 


INITIAL  FILTER  COVARIANCE  -  INS  PROBLEM 


Filter  Pf i i (0) 

S  tat  e, i  English  Units  Computati on  Units 


1 

2 

3 

4 

5 


(120  ft)**2 

(2  f ps) **2 
(0.1  deg)**2 

(0.01  deg / h  r ) **2 
(200  micro  g's)**2 


14400 

4 

3 .046E-6 
2 . 350E-1 5 
4 .147E-5 


f  t**2 
( f ps ) **2 
r  ad**2 

(rad/sec)**2 

(fps2)**2 


5 .1  .3  SOFE  Implementat i on 

The  subroutines  required  to  implement  the  foregoing 
problem  are  shown  at  the  beginning  of  Appendix  A.  The  input 
data  for  this  problem  were  previously  shown  in  Figure  4-2. 
Some  notes  about  this  implementation  follow: 


o  Total  impulsive  control  was  implemented  in  AMEND. 


o  Since  F  and  Qf  are  constant,  all  computations  for 
these  quantities  were  done  in  FQGEN0 . 
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o  The  INS  was  at  rest  on  the  earth's  surface,  so 
trajectory  computations  were  not  needed,  either 
internally  or  externally.  Thus  TRAJ  is  eliminated 
and  TRAJO  is  little  more  than  a  program  stub. 


o  Given  a  first  order  Markov  process  with  time  con¬ 
stant  T AU  and  steady  state  variance  SIGMA**2,  the 
increase  in  process  variance  over  an  interval  OT  ,  s 
(SIGMA**2)  *  ( 1  -  £  X  P  (2  *DT  /  T  AU  )  )  which  is  approximated 
for  small  DT/TAU  (in  SNOYS)  by  (SIGMA**2)  * 
(2*DT/TAU) 


o  USRIN  was  used  to  read  the  statistical  data  des¬ 
cribing  the  Markov  processes  and  the  measurement 
noises.  Two  NAMELISTS  were  set  up  for  this  purpose, 
one  for  the  filter  (INF)  and  one  for  the  truth 
(INS)  . 


o  Function  subroutine  GAUSS,  supplied  for  the  user's 
convenience  in  basic  SOFE,  was  used  in  HRZ,  in 
SNOYS,  and  in  XSDOT  to  obtain  random  Gaussian  sam¬ 
ples.  GAUSS  has  two  arguments,  the  first  being  the 
sample's  desired  mean  and  the  second  its  standard 
deviation.  GAUSS  was  used  in  HRZ  to  simulate  the 
measurement  noise  vf,  in  SNOYS  to  simulate  the 
change  in  Xs  due  to  driving  noise  ws,  and  in  XSDOT 
to  initialize  the  random  constant  state  s6 . 


o  The  problem  was  set  up  as  a  single  run  (IPASS=1) 
of  ten  hours  duration. 


5.1.4  Results  and  Conclusions 

Appendix  A  contains  the  printed  output  for  this  sample 
problem.  Note  that: 


o  The  data  echoed  on  the  early  pages  of  the  printout 
matches  that  prescribed  by  Figure  4-2. 


o  Printout  is  disabled  at  update  times  -  1200  up¬ 
dates  occurred  -  to  avoid  excessive  output. 
Periodic  output  occurs  only  at  one  hour  intervals. 
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o  A  total  of  five  plots  have  been  made.  The  plot 
titles  have  been  chosen  to  match  their  plot  parame¬ 
ter  set  input  (see  Table  4-4). 


o  The  last  page  of  printed  output  is  the  da/file 
showing  the  CDC  job  control  used  in  making  this  run. 
Note  the  parameters  on  the  first  card,  the  job  card, 
and  the  time  required  to  complete  the  run.  Note 
also  that  the  source  code,  including  the  user  pro¬ 
grams,  is  being  carried  in  CDC  "UPDATE"  format. 
Both  this  code  and  the  data  are  stored  as  permanent 
files  on  the  CDC  disc.  The  required  information  for 
accessing  these  files  is  given  on  the  ATTACH  caris. 


The  printer  plots  near  the  end  of  Appendix  A  show  that 
filter  states  Xf2,  Xf3,  Xf4  and  Xf5  track  the  corresponding 
truth  states  within  acceptable  covariance  limits.  This 
point  might  be  argued  in  the  case  of  state  Xf2  since  the 
true  error  is  occasionally  over  3  sigma  from  the  error  as 
estimated  by  Pf.  Since  state  Xf2  always  rights  itself  we 
have  chosen  to  label  its  performance  acceptable. 

However,  the  first  printer  plot  clearly  shows  that  X f 1 
does  not  track  Xsl  .  Pfll  shrinks  to  less  than  10  feet  in 
the  first  hour  (a  on  the  plot  indicates  coincident 
points,  in  this  case  between  2,  which  corresponds  to 
♦SQRT(Pfll)  and  3,  which  corresponds  to  -SQRT(Pfll))  while 
the  difference  Xsl-Xfl  (represented  by  curve  1)  meanders 
about  aimlessly.  Xs8,  being  a  large  unmodeled  bias  on  each 
position  measurement.  Is  probcbly  the  root  cause  of  diver¬ 
gence  in  Xf 1  . 
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Since  one  state  is  divergent,  the  filter  is  considered 
divergent  and  would  require  either  redesign  or  tuning  (ad¬ 
justment  of  parameters)  to  get  acceptable  performance. 

5.2  Non  linear  System  Example 

The  following  are  the  in-plane  equations  of  motion  for 
a  unit  mass  in  an  inverse  square  law  force  field: 

V  =  r»  -  Go /r  (5-7) 

0*  =  -2r0/r  (5-8) 

In  (5-7)  and  (5-8),  r  is  the  range  from  the  center  of  the 
force  field  to  the  unit  mass  (satellite)  and  0  is  the  angle 
between  the  unit  mass  and  a  reference  line  passing  through 
the  center  of  the  field  and  fixed  in  space.  For  conveni¬ 
ence,  we  let  Go  =  1  and  r(T0)  =  1.  In  addition,  if  r*(T0)  is 
zero,  the  forces  produced  at  TO  are  roughly  1/32  those  on 
earth's  surface.  The  general  solution  is  an  orbit  along 
some  conic  section,  two  versions  of  which  are  ar  circle  and 
an  ellipse.  Figure  5-2.  When  the  ellipse  is  a  transfer  path 
from  one  circular  orbit  of  radius  rl  to  another  of  radius  r2 
and  the  speed  change  required  to  achieve  the  transfer  is  dv, 
the  doub ly-tangent  elliptical  path  that  results  is  called 
the  Hohmann  transfer  orbit.  These  orbits  are  determined  by 
the  initial  conditions  on  <5-7)  and  (5-8). 
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Figure  5-2.  Satellite  Orbi 


Given  measurements  of  r  and  0,  the  problem  will  be  to 

•  • 

determine  the  position  (r,0)  and  velocity  (r,0)  of  the  sa¬ 
tellite.  This  example  is  an  adaptation  of  material  from 
Reference  6. 

5.2.1  Truth  Mode l 

Define  the  state  vector  Xs  as 


— 

X  s  1 

s 

r 

X  s2 

• 

r 

Xs3 

0 

X  s4  _ 

_  0  _ 

and  rewrite  (5-7)  and  (5-8)  in  state-space  form  as 


Xs  =  jj  (Xs,  t ) 


X  s2 

Xs1*Xs4**2  -  Go/Xsl **2 
X  s4 

-2*Xs2*Xs4/Xs1 


(5-10) 


Note  the  peculiar  absence  of  random  driving  terms  in  (5-10). 
Phenomena  such  as  solar  pressure,  atmospheric  drag,  gravity 
anomalies,  outgassing  and  satellite  tumbling  could  add  un¬ 
certainty  to  (5-10)  but  these  factors  have  been  ignored. 
The  truth  model  thereby  becomes  an  idealized  representation 
of  nature.  One  consequence  is  to  make  the  implementation  in 
SO F E  somewhat  easier:  (5-10)  provides  the  equations  for 
XSDOT  while  SNOYS  has  no  role  at  all.  Ground  observations 
of  r  and  0  are  available  every  0.5  time  units,  > 
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Zs(ti)  =[~Xs1(ti)  ♦  vsl(ti)"]  (5-11) 

|_X  s  3  ( 1 1 )  ♦  v  s  3  ( t  i  )J 

where  is  a  zero-mean  white  Gaussian  noise  having  a  diago¬ 
nal  2x2  covariance  matrix  Rs  given  by 

Rs  =  f  0.1 **2  0  ~] 

L  0  0 . 2  **2  _] 


5.2.2  Filter  Mode  l 

The  highly  nonlinear  nature  of  the  satellite  equations 
of  motion  suggests  very  strongly  that  the  filter  propagation 
equations  must  also  be  nonlinear.  Thus  the  following  equa¬ 
tions  were  adopted  to  model  the  satellite  motion  for  the 
filter: 


Xf  =  f (Xf,t)  +  wf  <t> 


- 

Xf  2 

♦ 

0 

Xf 1 *Xf4**2-Go/Xf  1  **2 

wfl 

X  f  4 

0 

-2*Xf2*Xf4/Xf  1 

wf  2 

The  states  in  Xf  correspond  one-for-one  to  those  in  Xs.  The 
distinction  between  (5-12)  and  (5-10)  is  that  the  two  random 
driving  processes  wfl  and  wf2  have  been  added  to  account  for 
orbit  uncertainties. 
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The  filter  measurement  equation  is 


i)  =f Xfl  (ti)  +  vfl  ( t  i )""] 
j_X  f  3  ( t  i )  +  v  f  3  ( t  i )  J 


(5-13) 


where  is  a  zero-mean  white  Gaussian  noise  having  a  diago¬ 
nal  2x2  covariance  matrix  Rf  given  by 


Rf  =  0.1 **2 

0 


0  1 

0 . 2 **2  J 


Since  the  filter  model  is  nonlinear,  the  extended  Kal¬ 
man  filter  will  be  used.  The  state  propagation  equation  is 


Xf  =  £(Xf,t) 


(5-14) 


where  _f(.)  is  given  by  (5-12).  In  order  to  keep  track  of 

a 

the  covariance  of  the  error  DX  in  Xf  ,  F  (t;Xf)  was 
established  using  (2-16): 


F(t; Xf )  = 


0  10  0 
Xf4**2+2Go/Xf1  **3  0  0  2Xf1*Xf4  (5-15) 

0  0  0  1 
2Xf 2*X  f4 /Xf 1 **2  -2Xf4/Xf 1  0  -2Xf2/Xf1 


Also,  based  on  (5-12),  Of  is 


(5-16) 
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The  nonzero  elements  in  (5-15)  and  (5-16)  are  used  to 


•  i 

form  P  =  FP  +  PF  +Qf  to  propagate  the  covariance. 


The  filter  processes  two  measurement  differences  at  up¬ 
date  time: 


=  fxsl  +  v s  1  -  *f1 

_X s3  ♦  vs3  -  X f 3_ 


(5-1 7) 


By  inspection  of  (5-17)  H  is 


H 


0  0  o' 
0  1  0_ 


(5-18) 


The  initial  covariance  matrix  Pfo  is  diagonal  with  values 
listed  in  the  table  below. 


Table  5-3 


INITIAL  FILTER  COVARIANCE  -  ORBIT  PROBLEM 


Fi Iter 

State/ i  Pfo  <  i# i  )  Units 


1 

0.1 

l engt  h**2 

2 

0.1 

(length/uni t 

time) +*2 

3 

0.1 

radi ans**2 

4 

0.1 

(radi ans/uni t 

time) **2 

5.2.3  SOFE  Implementat ion 

The  input  data  for  this  study  are  shown  in  Appendix  D. 
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The  subroutines  required  to  implement  this  nonlinear  problem 
are  included  with  the  listed  output  in  Appendix  B.  Some 
notes  about  this  implementation  follow: 


o  Since  subroutine  XPLUS  in  basic  SOFE'  computes  £f+ 
as 


Xf  +  =  Xf "  +  K*ZRES 


(5-19) 


the  whole-valued  estimate  is  completely  correct¬ 
ed  at  ti+  .  No  further  corrective  action  is  re¬ 
quired  so  AMEND  was  merely  a  RETURN  statement. 


o  Since  Qf  is  constant,  it  was  computed  once  for  all 
time  in  FQGENO. 

o  Since  the  truth  model  was  devoid  of  random  driving 
noise,  SNOYS  was  only  a  stub  in  the  load  module. 


o  As  with  the  linear  example,  no  trajectory  computa¬ 
tions  were  needed  (outside  those  in  XSDOT  and  XFDOT) 
so  TRAJ  is  not  required  at  all,  and  TRAJO  is  re¬ 
quired  only  as  a  stub. 


o  Two  different  truth  orbits  were  implemented  in 
this  study.  The  first  truth  orbit  was  circular 
while  the  second  was  elliptical  with  r2:r1  =  10:1. 
Since  initial  conditions  completely  determine  orbit, 
the  two  orbits  were  created  using  the  following 
values  at  TO:, 


(1  .  0.  0.  1.)  circular  orbit 

(1.  0.  0.  1.3483997245)  elliptical  orbit 


a 

The  above  values  initialized  both  Xs  and  £f.  The 
circular  amd  elliptical  orbit  studTes  were  Conducted 
In  separate  computer  runs.  The  output  In  Appendix  B 
and  the  job  control  in  Appendix  D  Illustrate  the 
circular  orbit  case. 
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o  Subroutine  ESTIX  was  constructed  to  compute  the 
average  s i g ma  (P f ( i , i ) **Q . 5 )  as  well  as  the  mean  and 
standard  deviation  of  true  error  for  an  ipnsemble  of 
Monte  Carlo  runs.  rue  error  in  state  Xf(i)  is  the 
difference  Xs(i)  -  Xf(i).  In  this  study,  sampling 
of  sigm3  and  true  error  values  occurred  at  DTSTIX  = 
0.5  time  units,  just  before  each  update,  on  all  four 
states.  When  T  reached  T F  on  the  last  run,  ESTIX 
computed  the  appropriate  statistics  for  each  state 
at  each  sample  time  for  the  ensemble  of  IPASS  runs. 
These  ensemble  statistics  are  printed  just  before 
the  printer  plots  in  Appendix  9. 


o  TF  was  5  time  units,  Qfl  was  0.02  l engt h2 / t i me3 , 
and  Qf2  was  0.02  rad2/time3. 


5.2.4  Results  and  Conclusions 

Before  viewing  results  for  the  problem  as  presented, 
consider  a  variation  in  which  just  the  angle  9  is  the  meas¬ 
ured  quantity.  This  was  tried  first,  unsuccessfully.  Some 
runs  worked  alright,  but  eventually  a  run  occurred  where  r 
would  shrink  to  nearly  zero,  causing  Pf  to  blow  up  -  note 
all  the  r's  in  the  denominators  of  (5-15).  At  first,  scal¬ 
ing  was  suspected,  e.g.  a  huge  force  field  (Go  too  large) 
or  an  unreasonably  small  r(T0),  but  changing  these  parame¬ 
ters  failed  to  relieve  the  problem.  Next,  measurement  accu¬ 
racy  was  improved,  but  the  blow-ups  continued.  Some  tinker¬ 
ing  with  Qfs  also  was  done,  but  the  blow-ups  remained.  In 
the  end,  some  performance  improvement  had  resulted  but  at¬ 
tempts  to  make  the  filter  work  with  just  6  measurements  were 
called  off  and  labeled  unsuccessful.  The  cause  of  this 
failure  is  the  weak  coupling  between  range  and  angle. 
Evidently  one  can  know  8  very  well  and  know  little  about  r. 
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even  though  the  system  is  theoretically  observable  with  only 
8  measurements  present. 

We  now  return  to  the  problem  as  presented.  Appendix  B 
begins  with  the  printed  output  for  the  circular  orbit  case. 
This  output  contains  a  listing  of  the  user-written  routines, 
printer  plots  and  periodic  data  for  the  first  run,  and 
statistics  for  the  50-run  study.  In  addition.  Figures  B-1 
through  B-8,  which  were  made  by  the  postprocessor  SOFEPL 
from  SOFE's  TAPE4  output,  depict  this  filter's  performance 
using  ensemble  averages. 

All  available  data  indicate  that  £f  tracks  X.s  within 
acceptable  bounds.  Figures  B-1  through  B-4  show  the  average 
of  true  estimation  error  e  surrounded  by  Se,  the  standard 
deviation  computed  from  e  data.  The  e  curve  in  these  four 
figures  is  approximately  zero  mean  as  it  should  be.  Note 
the  agreement  between  the  pair  e,S.e  as  displayed  in  the  SO¬ 
FEPL  curves  and  in  the  statistical  summary  near  tJie  end  of 
the  listing.  This  summary  certainly  provides  some 
quick-look  numbers,  but  the  cost  in  coding  of  ESTIX  and  the 
limited  visibility  that  raw  numbers  can  provide  work  in 
favor  of  using  SOFEPL  to  view  resutts  whenever  possible. 

Figures  B-5  through  B-8  each  contain  two  curves,  a 
solid  line  for  Se  and  a  broken  line  for  Sflftf (Pf i .  In  a  con- 
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servative  filter  design,  SQ T ( P f )  should  generally  be  some¬ 
what  greater  than  Se  so  that  the  filter  will  be  'open'  to 
the  variability  in  the  actual  system.  Figures  B-5  through 
B-8  show  that  this  dictum  has  not  been  consistently  satis¬ 
fied,  indicating  that,  at  a  minimum,  tuning  is  needed.  If 
tuning  is  neglected,  tracking  may  diverge  for  longer  orbits. 

The  study  described  above  was  repeated  using  10:1  el¬ 
liptical  orbit  initial  conditions  for  both  the  filter  and 
truth  models.  The  results  (not  shown  here)  were  similar  to 
those  for  the  circular  orbit  with  some  improvement  in  track- 
i ng  between  Se  and  SQRT(Pf). 

In  a  third  and  final  study  the  truth  orbit  was  the  10:1 
ellipse  ()(so  =  1,0,0,1.34...)  while  the  filter  state  was  in¬ 
itialized  with  these  values:  2. 0,0. 1,0. 2, 1.0  .  This  offset 
in  initial  conditions  is  within  the  1-sigma  bounds  pres¬ 
cribed  by  Pfo  for  all  states  except  r,  where  the  error  is 
roughly  3-sigma.  These  initial  offsets  produced  a  transient 
in  e  for  all  four  states  that  died  out  af'.er  cwo  time  units 
had  elapsed  and  four  measurements  were  processed.  Other 
measures  of  performance  were  essentially  unchanged  from  the 
previous  10:1  study. 

In  summary,  the  four-state  extended  filter  was  able  to 
estimate  actual  satellite  trajectories  despite  fairly  large 


110 


initial  condition  errors.  The  match  between  Se  and  SQRT(Pf) 


was  satisfactory  for  all  four  states  at  all  times  except  for 
the  circular  orbit  at  about  five  time  units.  A  problem  may 
be  surfacing  for  this  case,  but  it  was  not  pursued  here.  On 
balance,  Pfo,  Rf  and  Qf  appear  to  be  fairly  well  chosen  for 
the  range  of  missions  that  were  tried. 

5.3  Standard  Short  Test 

Appendix  C  shows  three  pages  of  printed  output  for  the 
linear  INS  problem  with  a  TF  of  61  seconds  instead  of  ten 
hours.  This  short  run  serves  as  a  standard  short  test  case 
for  SOFE.  Three  propagations  (0  to  30,  30  to  60,  60  to  61) 
and  two  updates  (at  30  and  60)  occur  in  the  61  seconds  of 
the  run,  thereby  exercising  all  of  SOFE’s  code  except  the 
printer  plotting  and  the  external  trajectory  capability. 
This  amounts  to  a  thorough  check  for  a  very  small  expendi¬ 
ture  of  computer  time  (less  than  two  seconds). 

Since  LPRUD,  LPRH,  IPRK  and  LPRZR  are  TRUE,  a  large 
amount  of  output  is  displayed  at  update  time.  Since  DTP R NT 
exceeds  61,  the  only  other  output  occurs  at  TO  and  Tf.  Note 
the  final  check  product  at  the  run's  conclusion: 

-0.132880246897869  E-143 

This  number  provides  a  quick  and  handy  check  of  'duplicate' 
test  cases. 


Ill 
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STATISTICS  AFTER  50  RUMS  OF  CIRCULAR  ORBIT 
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APPENDIX  D 
Job  Control 

SOFE  was  developed  on  the  CDC  CYBER-74  computer  system 
at  Wri ght-Patterson  AFB,  Ohio,  using  the  NOS/BE  operating 
system.  The  central  memory  requirements  of  SOFE  are  usually 
too  large  for  it  to  run  interactively  on  this  system  so  var¬ 
ious  methods  of  batch-entry  job  control  have  been  devised. 
These  methods  involve  considerations  of  both  program  and 
data  manipulation  which  in  this  case  includes  manipulation 
of:  1)  basic  SOFE;  2)  user-written  SOFE;  and  3)  the 
numerical  data.  Two  possible  methods  for  job  control  will 
be  demonstrated  here  in  Figures  D-1  and  D-2 .  Figure  D-1  is 
the  permanent  file  approach  while  D-2  is  the  card  input  ap¬ 
proach. 

Figure  D-1  is  the  job  control  for  the  linear  system  ex¬ 
ample  of  Section  5.1.  Figure  D-1  illustrates  job  control 
setup  and  SOFE  use  when:  1)  Basic  SOFE  and  the  user-written 
subroutines  are  on  one  local  file  named  'OLDPL 1  in  the  CDC 
UPDATE  format;  2)  the  numerical  data  are  on  another  local 
file  named  TAPES.  Both  OLDPL  and  TAPE5  are  permanently 
stored  on  disk,  OLDPL  being  in  a  perm  file  named  SOFE  and 
TAPE5  being  in  a  perm  file  named  SOFEDATA.  To  create  an  ob¬ 
ject  module,  a  full  update  is  performed  on  the  OLDPL  file 
thereby  producing  a  card-image  file  called  COMPILE  which  is 
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then  compiled.  With  this  complete  object  module  plus  the 
TAPES  data,  SOFE  execution  can  commence. 

Figure  D-2  is  the  job  control  for  the  nonlinear  system 
example  of  Section  5.2.  Figure  D-2  illustrates  deck  setup 
and  SOFE  use  when:  1)  basic  SOFE  is  part  of  an  UPDATE  file 
named  OLDPL;  2)  the  user-written  subroutines  are  on  cards 
following  the  JCL;  3)  the  numerical  data  are  on  cards  fol¬ 
lowing  the  user-written  subroutines.  Selective  update  of 
basic  SOFE  is  illustrated  in  the  JCL  using  the 
' * C  SOFE.ZROIZE'  data  card.  Following  compilation  of  basic 
SOFE,  a  compilation  of  the  user-written  subroutines  occurs. 
Both  object  modules  end  up  on  the  file  named  L60 . 

The  numerical  input  (problem  title,  PR  D  AT  A  group,  etc.) 
for  the  Figure  D-2  example  is  at  the  end  of  the  card  deck  on 
the  INPUT  file.  Since  SOFE  expects  its  input  on  TAPE5, 
INPUT  must  be  equated  to  TAPE5  at  load  time.  This  can  be 
done  under  NOS/BE  by  inserting  the  name  INPUT  in  the  loca¬ 
tion  reserved  for  TAPES  on  the  LGO  card,  viz.  the  first  lo¬ 
cation.  For  reference,  all  file  assignments  are  shown  on 
the  LGO  card  in  Figure  D-2  even  though  only  the  first  re¬ 
placement  is  needed  in  this  case. 


SHM,T35,CM75Q0Q.  V7201 30,MUS I CK 
COMMENT  .* 

COMMENT .  *  STANDARD  LONG  TEST  FOR  'SOFE' 

COMMENT  .* 

COMMENT  .*  ATTACH  AND  COMPILE  BASIC  SOFE  WITH 
COMMENT.  USER-WRITTEN  ROUTINES  APPENDED. 

ATTACH ,OLDPL, SOFE , C Y=999 , I D= S HM, SN= A F AL ,MR = 1 . 
UPDATE, F,C=COMPILE,0=OUTPUT  . 

FTN,I=COMPILE ,  L  =  0 . 

RE T URN, OL DP L, COMPILE . 

COMMENT  .  * 

COMMENT . *  ATTACH  CARD  INPUT  DATA  AND  RUN  SOFE. 
ATTACH, TAP£5,SOF£DATA,CY=222,ID=SHM,SN=AFAL, MR =1 . 
LGO . 

*E  OR 


Figure  D-1 .  Job  Control,  All  Files  on  Disk 
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SHM,T60, 10100, CM70000.  V720130,  MUSICK 

COMMENT . 

COMMENT.*  1 SOFE '  EXAMPLE. 

COMMENT.*  EXTENDED  KALMAN  FILTER. 

COMMENT.*  NONLINEAR  SATELLITE  ORBIT  PROBLEM. 

COMMENT . 

COMMENT.*  SINGLE  DATA  CARD  FOR  FOLLOWING  UPDATE 
COMMENT.*  READS  "*C  SO  FE  .  ZRO  I  ZE . 

ATTACH,OLDPL,SOFE ,CY=999,ID=SHM,SN=AFAL,MR=1  . 

UPDATE ,Q,C=COMPILE . 

COMMENT  . 

COMMENT.*  COMPILE  BASIC  SOFE  SUBROUTINES. 

FTN,I=COMPILE,L=0. 

RETURN, OLDPL, COMPILE . 

COMMENT  . 

COMMENT.*  COMPILE  USER-WRITTEN  SUBROUTINES. 

FTN,I=INPUT,R=0,P  . 

COMMENT  . 

COMMENT.*  RUN  SOFE  USING  TAPES  DATA  ON  INPUT  FILE. 

REQUEST, TAPE4, *PF  . 

LGO (INPUT, TAPE3,TAPE9,0UTPUT ,TAPE4, OUTPUT, TAPE8,TAPE10, TAPE?) . 
COMMENT . 

COMMENT.*  SAVE  TAPE4  FOR  LATER  PLOTTING. 

CATALOG, TAP£4,S0FE0RBITPL0TTAPE,ID=SHM. 

7/8/9 

*C  SOFE.ZROIZE 
7/8/9 

(USER-WRITTEN  SUBROUTINES  GO  HERE.  SEE  APPENDIX  B) 

7/8/9 

SATELLITE  ORBIT  DETERMINATION  USING  AN  EXTENDED  KALMAN  FILTER 
SPRDATA  N  F=4 ,  NS  =  4,  M=2,  NZF  =  7,  NZQ=2,  ISEED  =  23,  IPGSIZ=55, 

T  F=5 . 0 ,  DTPRPL  =  0 .05 ,  DTCCPL  =  0.05,  DTMEAS=0.5,  DTSTIX=0.5, 

IPASS  =  50,  IPRRUN= 1 ,  LPRZR*  .T  .  ,  LP  R  LT=  .  T . ,  LPP=.T.,  LCC=.T.,  S 
1,2,  2,1,  2,4,  3,4,  4,1,  4,2,  4,4  /  2,2,  4,4 
1.,0.,0.,1.  /  1.,0.,0.,1. 

1  ,1 ,  - 1  /  2,2,  .1  /  3,3,  .1  /  4,4,  .1  /  0,0,  .0 
SIN  RFIN (1 )=0.01 ,0 .04,  QFIN(1)  =  2*0.02,  G0=1 .0  S 

1  .0 

1  r  1  • 

2r,2,2,1  . 

3.3.3.57.2957795 

4.4.4.57.2957795 
0,0, 0,1 . 

TIME  (TIME  UNITS) 

TRUE  RANGE  ERROR  — >  1  :  +SIGMA  -->  2  :  -SIGMA  -->  3 
RANGE  *LENGTH * 

TRUE  RANGE  RATE  ERROR  -->  1  :  +SIGMA  — >  2  :  -SIGMA  -->  3 
RANGE  RATE*LEN/UNIT  TIME* 

TRUE  ANGLE  ERROR  — >  1  :  +SIGMA  — >  2  :  -SIGMA  — >  3 
ANGLE  *D£GRE  ES* 

TRUE  ANGLE  RATE  ERROR  -->  1  :  ♦ SIGMA  -->  2  :  -SIGMA  — >  3 
ANGLE  R ATE*DEG/UNIT  TIME* 

7/8/9 

6/7/8Z9 

Figure  0-2.  Job  Control,  All  User  Input  on  Cards 
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Errata  for  AD  A093887 
Feb  82 

SOFE:  A  Generalized  Digital  Simulation  for  Optimal 
Filter  Evaluation,  User's  Manual,  Oct  1980 


1.  Page  25,  third  line  from  bottom.  Correct  this  line  to  read 

=  E  {  (Xf-Xf)  (Xf-Xf)T  }  by  (2-18) 

2.  Page  46.  Subroutine  'goplot'  has  been  rewritten  and  now  calls 

subroutine  'scale'  for  scaling  of  the  y  and  t  axes, 
'scale'  should  be  shown  feeding  into  'goplot'  in  the 
flowchart. 

3.  Page  48,  lines  11  and  12  from  top.  These  two  lines  note 

exceptions  to  ANSI  standard  practice  that  were  employed 
'in  GOPLOT  only'.  The  revised  GOPLOT  avoids  these 
practices  so  these  lines  can  be  ignored. 

4.  Page  101,  eq  (5-7).  Correct  eq  (5-7)  to  read 

r  =  rQ2  -  G0/r2  (5-7) 

Note  that  the  error  being  corrected  here  appeared  in  the 
SOFE  manual  but  not  in  the  computer  code;  therefore  the 
satellite  orbit  results  are  correct  as  they  appear  in 
the  manual. 

5.  Page  111,  third  line  from  bottom.  New  check  product  reflects 

correction  of  errata  item  6. 

-0.208788421550344  E-114 

6.  Page  116,  Subroutine  FQGEN.  Change  F(l)  to  agree  with  eq  (5-4). 

F(l)  -  1. 

Note  that  correction  6  causes  a  substantial  increase  in 
the  sigma  of  filter  state  1  (at  T=36000,  old  2.88592,  new 
36,3497)  and  affects  other  states  also  but  not  nearly 
as  much.  The  conclusion  that  the  INS  Kalman  filter 
diverges  is  still  correct. 


